openpilot v0.4.0.1 release

old-commit-hash: a77c0a1098
commatwo_master
Vehicle Researcher 8 years ago
parent 680ec1792b
commit d9578e2f8e
  1. 19
      README.md
  2. 9
      RELEASES.md
  3. 3
      apk/ai.comma.plus.black.apk
  4. 3
      apk/ai.comma.plus.frame.apk
  5. 3
      apk/ai.comma.plus.offroad.apk
  6. 3
      apk/com.baseui.apk
  7. 7
      cereal/car.capnp
  8. 41
      cereal/log.capnp
  9. 2
      common/basedir.py
  10. 3
      common/fingerprints.py
  11. 6
      common/kalman/ekf.py
  12. 3
      common/params.py
  13. 49
      common/profiler.py
  14. 39
      launch_openpilot.sh
  15. 3
      selfdrive/assets/OpenSans-Regular.ttf
  16. 3
      selfdrive/assets/OpenSans-SemiBold.ttf
  17. 4
      selfdrive/boardd/boardd.cc
  18. 3
      selfdrive/can/libdbc_py.py
  19. 2
      selfdrive/can/parser.cc
  20. 22
      selfdrive/car/__init__.py
  21. 2
      selfdrive/car/honda/carcontroller.py
  22. 1
      selfdrive/car/honda/carstate.py
  23. 12
      selfdrive/car/honda/interface.py
  24. 0
      selfdrive/car/mock/__init__.py
  25. 121
      selfdrive/car/mock/interface.py
  26. 56
      selfdrive/car/toyota/carcontroller.py
  27. 18
      selfdrive/car/toyota/carstate.py
  28. 36
      selfdrive/car/toyota/interface.py
  29. 56
      selfdrive/car/toyota/values.py
  30. 2
      selfdrive/common/version.h
  31. 155
      selfdrive/controls/controlsd.py
  32. 281
      selfdrive/controls/lib/alertmanager.py
  33. 1
      selfdrive/controls/lib/drive_helpers.py
  34. 15
      selfdrive/controls/lib/latcontrol.py
  35. 40
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  36. 13
      selfdrive/controls/lib/lateral_mpc/libmpc_py.py
  37. 18
      selfdrive/controls/lib/lateral_mpc/mpc.c
  38. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
  39. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
  40. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp
  41. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
  42. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
  43. 4
      selfdrive/controls/lib/longcontrol.py
  44. 30
      selfdrive/controls/lib/longitudinal_mpc/generator.cpp
  45. 17
      selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
  46. 19
      selfdrive/controls/lib/longitudinal_mpc/mpc.c
  47. 4
      selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h
  48. 4
      selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c
  49. 2
      selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp
  50. 2
      selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp
  51. 4
      selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c
  52. 40
      selfdrive/controls/lib/planner.py
  53. 5
      selfdrive/controls/lib/speed_smoother.py
  54. 2
      selfdrive/controls/lib/vehicle_model.py
  55. 73
      selfdrive/controls/radard.py
  56. 2
      selfdrive/loggerd/loggerd
  57. 16
      selfdrive/loggerd/uploader.py
  58. 170
      selfdrive/manager.py
  59. 0
      selfdrive/radar/mock/__init__.py
  60. 24
      selfdrive/radar/mock/interface.py
  61. 1
      selfdrive/radar/nidec/interface.py
  62. 1
      selfdrive/radar/toyota/interface.py
  63. 11
      selfdrive/registration.py
  64. 4
      selfdrive/sensord/gpsd
  65. 2
      selfdrive/sensord/sensord
  66. 2
      selfdrive/service_list.yaml
  67. 1
      selfdrive/test/plant/maneuver.py
  68. 11
      selfdrive/test/plant/maneuverplots.py
  69. 8
      selfdrive/test/test_openpilot.py
  70. 1
      selfdrive/ui/Makefile
  71. 725
      selfdrive/ui/ui.c
  72. 3
      selfdrive/updated.py
  73. 4
      selfdrive/visiond/visiond

@ -3,7 +3,7 @@ Welcome to openpilot
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. [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. 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 - Honda CR-V Touring 2015-2016
- Can only be enabled above 25 mph - 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 - 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!) - Toyota Prius 2017 (alpha!)
- By default it uses stock Toyota ACC for longitudinal control - 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 In Progress Cars
------ ------
@ -50,6 +55,12 @@ Community WIP Cars
- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145) - [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 Directory structure
------ ------

@ -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) Version 0.3.9 (2017-11-21)
========================== ==========================
* Add alpha support for 2017 Toyota Prius * Add alpha support for 2017 Toyota Prius

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6e6f3a8037283b87271a7bb0c95a9ae1a8ff5a86dea7e2b25d49911179b6e05e
size 84675

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:85eec719a82974770a70fec2c8cf47b12f395f93be39f4e11f50918622170f54
size 2077093

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:441c03e9bcb881ecc7933872d31d9dc7734a8673f3898fc8d8c49c6a15875b72
size 15382430

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6a4fc195fb4160a445c9e5b1d1aa4e37bcf8c3271890352da8b33524893dfa1c
size 6338698

@ -12,12 +12,13 @@ $Java.outerClassname("Car");
struct CarEvent @0x9b1657f34caf3ad3 { struct CarEvent @0x9b1657f34caf3ad3 {
name @0 :EventName; name @0 :EventName;
enable @1 :Bool; enable @1 :Bool;
preEnable @7 :Bool;
noEntry @2 :Bool; noEntry @2 :Bool;
warning @3 :Bool; warning @3 :Bool;
userDisable @4 :Bool; userDisable @4 :Bool;
softDisable @5 :Bool; softDisable @5 :Bool;
immediateDisable @6 :Bool; immediateDisable @6 :Bool;
preEnable @7 :Bool;
permanent @8 :Bool;
enum EventName @0xbaa8c5d505f727de { enum EventName @0xbaa8c5d505f727de {
# TODO: copy from error list # TODO: copy from error list
@ -66,7 +67,8 @@ struct CarState {
# car speed # car speed
vEgo @1 :Float32; # best estimate of speed vEgo @1 :Float32; # best estimate of speed
aEgo @16 :Float32; # best estimate of acceleration aEgo @16 :Float32; # best estimate of acceleration
vEgoRaw @17 :Float32; # unfiltered speed vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors
yawRate @22 :Float32; # best estimate of yaw rate
standstill @18 :Bool; standstill @18 :Bool;
wheelSpeeds @2 :WheelSpeeds; wheelSpeeds @2 :WheelSpeeds;
@ -309,4 +311,5 @@ struct CarParams {
directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake 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 stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
startAccel @35 :Float32; # Required acceleraton to overcome creep braking startAccel @35 :Float32; # Required acceleraton to overcome creep braking
steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
} }

@ -28,6 +28,7 @@ struct InitData {
version @4 :Text; version @4 :Text;
gitCommit @10 :Text; gitCommit @10 :Text;
gitBranch @11 :Text; gitBranch @11 :Text;
gitRemote @13 :Text;
androidBuildInfo @5 :AndroidBuildInfo; androidBuildInfo @5 :AndroidBuildInfo;
androidSensors @6 :List(AndroidSensor); androidSensors @6 :List(AndroidSensor);
@ -154,6 +155,8 @@ struct SensorEventData {
pressure @9 :SensorVec; pressure @9 :SensorVec;
magneticUncalibrated @11 :SensorVec; magneticUncalibrated @11 :SensorVec;
gyroUncalibrated @12 :SensorVec; gyroUncalibrated @12 :SensorVec;
proximity @13: Float32;
light @14: Float32;
} }
source @8 :SensorSource; source @8 :SensorSource;
@ -242,8 +245,11 @@ struct ThermalData {
freeSpace @7 :Float32; freeSpace @7 :Float32;
batteryPercent @8 :Int16; batteryPercent @8 :Int16;
batteryStatus @9 :Text; batteryStatus @9 :Text;
usbOnline @12 :Bool;
fanSpeed @10 :UInt16; fanSpeed @10 :UInt16;
started @11 :Bool;
startedTs @13 :UInt64;
} }
struct HealthData { struct HealthData {
@ -349,6 +355,7 @@ struct Live100Data {
jerkFactor @12 :Float32; jerkFactor @12 :Float32;
angleSteers @13 :Float32; # Steering angle in degrees. angleSteers @13 :Float32; # Steering angle in degrees.
angleSteersDes @29 :Float32; angleSteersDes @29 :Float32;
curvature @37 :Float32; # path curvature from vehicle model
hudLeadDEPRECATED @14 :Int32; hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32; cumLagMs @15 :Float32;
@ -361,6 +368,8 @@ struct Live100Data {
rearViewCam @23 :Bool; rearViewCam @23 :Bool;
alertText1 @24 :Text; alertText1 @24 :Text;
alertText2 @25 :Text; alertText2 @25 :Text;
alertStatus @38 :AlertStatus;
alertSize @39 :AlertSize;
awarenessStatus @26 :Float32; awarenessStatus @26 :Float32;
angleOffset @27 :Float32; angleOffset @27 :Float32;
@ -378,6 +387,20 @@ struct Live100Data {
stopping @2; stopping @2;
starting @3; 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 { 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 { struct CellInfo {
timestamp @0 :UInt64; timestamp @0 :UInt64;
repr @1 :Text; # android toString() for now repr @1 :Text; # android toString() for now
@ -1300,5 +1340,6 @@ struct Event {
clocks @35 :Clocks; clocks @35 :Clocks;
liveMpc @36 :LiveMpcData; liveMpc @36 :LiveMpcData;
liveLongitudinalMpc @37 :LiveLongitudinalMpcData; liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
navStatus @38 :NavStatus;
} }
} }

@ -1,4 +1,4 @@
import os 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__)), "../"))

@ -19,6 +19,9 @@ _FINGERPRINTS = {
"TOYOTA RAV4 2017": { "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 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": { "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 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
}, },

@ -35,7 +35,7 @@ class SensorReading:
# A generic sensor class that does no pre-processing of data # A generic sensor class that does no pre-processing of data
class SimpleSensor: class SimpleSensor:
# obs_model can be # 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 # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be # covar can be
# a full covariance matrix # a full covariance matrix
@ -129,7 +129,7 @@ class EKF:
print "covar:\n",self.covar print "covar:\n",self.covar
def update_scalar(self, reading): 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 # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state) # 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 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(), 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
""" """

@ -52,12 +52,14 @@ keys = {
"Version": TxType.PERSISTANT, "Version": TxType.PERSISTANT,
"GitCommit": TxType.PERSISTANT, "GitCommit": TxType.PERSISTANT,
"GitBranch": TxType.PERSISTANT, "GitBranch": TxType.PERSISTANT,
"GitRemote": TxType.PERSISTANT,
# written: baseui # written: baseui
# read: ui, controls # read: ui, controls
"IsMetric": TxType.PERSISTANT, "IsMetric": TxType.PERSISTANT,
"IsRearViewMirror": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT,
"IsFcwEnabled": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT,
"HasAcceptedTerms": TxType.PERSISTANT, "HasAcceptedTerms": TxType.PERSISTANT,
"IsUploadVideoOverCellularEnabled": TxType.PERSISTANT,
# written: visiond # written: visiond
# read: visiond, controlsd # read: visiond, controlsd
"CalibrationParams": TxType.PERSISTANT, "CalibrationParams": TxType.PERSISTANT,
@ -69,6 +71,7 @@ keys = {
"CarParams": TxType.CLEAR_ON_CAR_START, "CarParams": TxType.CLEAR_ON_CAR_START,
"Passive": TxType.PERSISTANT, "Passive": TxType.PERSISTANT,
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
} }
def fsync_dir(path): def fsync_dir(path):

@ -1,32 +1,47 @@
from common.realtime import sec_since_boot import time
class Profiler(object): class Profiler(object):
def __init__(self, enabled=False): def __init__(self, enabled=False):
self.enabled = enabled self.enabled = enabled
self.cp = [] self.cp = {}
self.start_time = sec_since_boot() self.cp_ignored = []
self.iter = 0
self.start_time = time.clock()
self.last_time = self.start_time self.last_time = self.start_time
self.tot = 0.
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
def reset(self, enabled=False): def reset(self, enabled=False):
self.enabled = enabled self.enabled = enabled
self.cp = [] self.cp = {}
self.start_time = sec_since_boot() self.cp_ignored = []
self.iter = 0
self.start_time = time.clock()
self.last_time = self.start_time 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): def display(self):
if not self.enabled: if not self.enabled:
return return
self.iter += 1
print "******* Profiling *******" print "******* Profiling *******"
tot = 0.0 for n in self.cp:
for n, ms in self.cp: ms = self.cp[n]
print "%30s: %7.2f" % (n, ms*1000.0) if n in self.cp_ignored:
tot += ms print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED"
print " TOTAL: %7.2f" % (tot*1000.0) 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)

@ -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

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:13c03e22a633919beb2847c58c8285fb8a735ee97097d7c48fd403f8294b05f8
size 217276

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b4c2050b25d3d296d5cf58589ca00816dc72df42262c2f629d5c6a984a161aa4
size 221164

@ -130,7 +130,11 @@ bool usb_connect() {
#endif #endif
// no output is the default // no output is the default
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); libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
}
if (safety_setter_thread_handle == -1) { if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);

@ -1,11 +1,12 @@
import os import os
import sys
import subprocess import subprocess
from cffi import FFI from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__)) can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so") 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 = FFI()
ffi.cdef(""" ffi.cdef("""

@ -242,7 +242,7 @@ class CANParser {
const auto& state = kv.second; const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) { if (state.seen > 0) {
INFO("%X TIMEOUT\n", state.address); DEBUG("%X TIMEOUT\n", state.address);
} }
can_valid = false; can_valid = false;
} }

@ -6,12 +6,9 @@ from common.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from .honda.interface import CarInterface as HondaInterface from selfdrive.car.honda.interface import CarInterface as HondaInterface
from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
try: from selfdrive.car.mock.interface import CarInterface as MockInterface
from .toyota.interface import CarInterface as ToyotaInterface
except ImportError:
ToyotaInterface = None
try: try:
from .simulator.interface import CarInterface as SimInterface from .simulator.interface import CarInterface as SimInterface
@ -31,9 +28,12 @@ interfaces = {
"HONDA CR-V 2016 TOURING": HondaInterface, "HONDA CR-V 2016 TOURING": HondaInterface,
"TOYOTA PRIUS 2017": ToyotaInterface, "TOYOTA PRIUS 2017": ToyotaInterface,
"TOYOTA RAV4 2017": ToyotaInterface, "TOYOTA RAV4 2017": ToyotaInterface,
"TOYOTA RAV4 2017 HYBRID": ToyotaInterface,
"simulator": SimInterface, "simulator": SimInterface,
"simulator2": Sim2Interface "simulator2": Sim2Interface,
"mock": MockInterface
} }
# **** for use live only **** # **** for use live only ****
@ -75,11 +75,17 @@ def fingerprint(logcan, timeout):
return (candidate_cars[0], finger) 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) candidate, fingerprints = fingerprint(logcan, timeout)
if candidate is None: if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
if passive:
candidate = "mock"
else:
return None, None return None, None
interface_cls = interfaces[candidate] interface_cls = interfaces[candidate]

@ -122,7 +122,7 @@ class CarController(object):
GAS_MAX = 1004 GAS_MAX = 1004
BRAKE_MAX = 1024/4 BRAKE_MAX = 1024/4
if CS.civic: 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 STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv: elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value

@ -1,5 +1,4 @@
import os import os
import time
from cereal import car from cereal import car
from common.numpy_fast import interp from common.numpy_fast import interp
import selfdrive.messaging as messaging import selfdrive.messaging as messaging

@ -1,11 +1,11 @@
#!/usr/bin/env python #!/usr/bin/env python
import os import os
import time
import numpy as np import numpy as np
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV 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.drive_helpers import create_event, EventTypes as ET, get_events
from selfdrive.controls.lib.vehicle_model import VehicleModel
from cereal import car from cereal import car
from selfdrive.services import service_list from selfdrive.services import service_list
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
@ -93,6 +93,7 @@ class CarInterface(object):
# *** init the major players *** # *** init the major players ***
self.CS = CarState(CP) self.CS = CarState(CP)
self.VM = VehicleModel(CP)
# sending if read only is False # sending if read only is False
if sendcan is not None: if sendcan is not None:
@ -163,7 +164,7 @@ class CarInterface(object):
ret.centerToFront = centerToFront_civic ret.centerToFront = centerToFront_civic
ret.steerRatio = 13.0 ret.steerRatio = 13.0
# Civic at comma has modified steering FW, so different tuning for the Neo in that car # 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.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpBP = [0., 5., 35.]
@ -252,6 +253,8 @@ class CarInterface(object):
ret.steerLimitAlert = True ret.steerLimitAlert = True
ret.startAccel = 0.5 ret.startAccel = 0.5
ret.steerRateCost = 0.5
return ret return ret
# returns a car.CarState # returns a car.CarState
@ -270,6 +273,7 @@ class CarInterface(object):
ret.vEgo = self.CS.v_ego ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw 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.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@ -370,11 +374,11 @@ class CarInterface(object):
else: else:
self.can_invalid_count = 0 self.can_invalid_count = 0
if self.CS.steer_error: 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: elif self.CS.steer_not_allowed:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error: 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': if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed: if not self.CS.door_all_closed:

@ -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

@ -6,7 +6,7 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \ create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \ create_ipas_steer_command, create_accel_command, \
create_fcw_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 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, 0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
0x383] 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): def accel_hysteresis(accel, accel_steady, enabled):
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command

@ -1,5 +1,4 @@
import os import os
import time
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from selfdrive.car.toyota.values import CAR from selfdrive.car.toyota.values import CAR
from selfdrive.can.parser import CANParser from selfdrive.can.parser import CANParser
@ -19,7 +18,7 @@ def parse_gear_shifter(can_gear, car_fingerprint):
return "drive" return "drive"
elif can_gear == 0x4: elif can_gear == 0x4:
return "brake" return "brake"
elif car_fingerprint == CAR.RAV4: elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]:
if can_gear == 0x20: if can_gear == 0x20:
return "park" return "park"
elif can_gear == 0x10: elif can_gear == 0x10:
@ -47,6 +46,17 @@ def get_can_parser(CP):
(550, 40), (550, 40),
(581, 33) (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: elif CP.carFingerprint == CAR.RAV4:
dbc_f = 'toyota_rav4_2017_pt.dbc' dbc_f = 'toyota_rav4_2017_pt.dbc'
signals = [ signals = [
@ -131,6 +141,10 @@ class CarState(object):
can_gear = cp.vl[295]['GEAR'] can_gear = cp.vl[295]['GEAR']
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
self.pedal_gas = cp.vl[581]['GAS_PEDAL'] 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: elif self.car_fingerprint == CAR.RAV4:
can_gear = cp.vl[956]['GEAR'] can_gear = cp.vl[956]['GEAR']
self.brake_pressed = cp.vl[548]['BRAKE_PRESSED'] self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']

@ -1,20 +1,24 @@
#!/usr/bin/env python #!/usr/bin/env python
import os import os
import time
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
import common.numpy_fast as np 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 cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list from selfdrive.services import service_list
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event 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): class CarInterface(object):
def __init__(self, CP, sendcan=None): def __init__(self, CP, sendcan=None):
self.CP = CP self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0 self.frame = 0
self.can_invalid_count = 0 self.can_invalid_count = 0
@ -70,9 +74,8 @@ class CarInterface(object):
tireStiffnessFront_civic = 85400 tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000 tireStiffnessRear_civic = 90000
stop_and_go = True
ret.mass = 3045./2.205 + std_cargo 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.centerToFront = ret.wheelbase * 0.44
ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
ret.steerKp, ret.steerKi = 0.6, 0.05 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 # 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. # 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. 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 ret.minEnableSpeed = 19. * CV.MPH_TO_MS
centerToRear = ret.wheelbase - ret.centerToFront centerToRear = ret.wheelbase - ret.centerToFront
@ -131,6 +134,11 @@ class CarInterface(object):
ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36] 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 return ret
# returns a car.CarState # returns a car.CarState
@ -150,6 +158,7 @@ class CarInterface(object):
ret.vEgo = self.CS.v_ego ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego 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.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@ -180,6 +189,11 @@ class CarInterface(object):
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0. ret.cruiseState.speedOffset = 0.
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 ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
# TODO: button presses # TODO: button presses
@ -223,8 +237,8 @@ class CarInterface(object):
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error: if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.low_speed_lockout: if self.CS.low_speed_lockout and self.CP.enableDsu:
events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY])) events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu:
events.append(create_event('speedTooLow', [ET.NO_ENTRY])) events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if c.actuators.gas > 0.1: if c.actuators.gas > 0.1:

@ -1,8 +1,64 @@
class CAR: class CAR:
PRIUS = "TOYOTA PRIUS 2017" PRIUS = "TOYOTA PRIUS 2017"
RAV4H = "TOYOTA RAV4 2017 HYBRID"
RAV4 = "TOYOTA RAV4 2017" RAV4 = "TOYOTA RAV4 2017"
class ECU: class ECU:
CAM = 0 # camera CAM = 0 # camera
DSU = 1 # driving support unit DSU = 1 # driving support unit
APGS = 2 # advanced parking guidance system 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)

@ -1 +1 @@
#define COMMA_VERSION "0.3.9-openpilot" #define COMMA_VERSION "0.4.0.1-openpilot"

@ -2,7 +2,7 @@
import json import json
from copy import copy from copy import copy
import zmq import zmq
from cereal import car from cereal import car, log
from common.numpy_fast import clip from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler 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_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 AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
State = log.Live100Data.ControlState
class Calibration: class Calibration:
UNCALIBRATED = 0 UNCALIBRATED = 0
@ -37,38 +38,43 @@ class Calibration:
INVALID = 2 INVALID = 2
class State:
DISABLED = 'disabled'
ENABLED = 'enabled'
PRE_ENABLED = 'preEnabled'
SOFT_DISABLING = 'softDisabling'
# True when actuators are controlled # True when actuators are controlled
def isActive(state): def isActive(state):
return state in [State.ENABLED, State.SOFT_DISABLING] return state in [State.enabled, State.softDisabling]
# True if system is engaged # True if system is engaged
def isEnabled(state): 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 *** # *** read can and compute car states ***
CS = CI.update(CC) CS = CI.update(CC)
events = list(CS.events) 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 checking logic ***
# thermal data, checked every second # thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None: if td is not None:
# overtemp above 95 deg # CPU overtemp above 95 deg
overtemp = any( overtemp_proc = any(t > 950 for t in
t > 950 (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) 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 # under 15% of space free no enable allowed
free_space = td.thermal.freeSpace < 0.15 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])) events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# *** read calibration status *** # *** read calibration status ***
cal = messaging.recv_sock(cal)
if cal is not None: if cal is not None:
cal_status = cal.liveCalibration.calStatus 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])) events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# *** health checking logic *** # *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None: if hh is not None:
controls_allowed = hh.health.controlsAllowed controls_allowed = hh.health.controlsAllowed
if not controls_allowed: 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 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 # 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 # 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: for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed: if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise": 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 ***** # ***** handle state transitions *****
# DISABLED # DISABLED
if state == State.DISABLED: if state == State.disabled:
if get_events(events, [ET.ENABLE]): if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
for e in get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled) AM.add(str(e) + "NoEntry", enabled)
else: else:
if get_events(events, [ET.PRE_ENABLE]): if get_events(events, [ET.PRE_ENABLE]):
state = State.PRE_ENABLED state = State.preEnabled
else: else:
state = State.ENABLED state = State.enabled
AM.add("enable", enabled) AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active # 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))) v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# ENABLED # ENABLED
elif state == State.ENABLED: elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]): if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED state = State.disabled
AM.add("disable", enabled) AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]): elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]): for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled) AM.add(e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]): elif get_events(events, [ET.SOFT_DISABLE]):
state = State.SOFT_DISABLING state = State.softDisabling
soft_disable_timer = 300 # 3s TODO: use rate soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]): for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled) AM.add(e, enabled)
# SOFT DISABLING # SOFT DISABLING
elif state == State.SOFT_DISABLING: elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]): if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED state = State.disabled
AM.add("disable", enabled) AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]): elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]): for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled) AM.add(e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]): elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED # no more soft disabling condition, so go back to ENABLED
state = State.ENABLED state = State.enabled
elif soft_disable_timer <= 0: elif soft_disable_timer <= 0:
state = State.DISABLED state = State.disabled
# TODO: PRE ENABLING # PRE ENABLING
elif state == State.PRE_ENABLED: elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]): if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED state = State.disabled
AM.add("disable", enabled) AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): 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]): for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled) AM.add(e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]): 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, def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators # Given the state, this function returns the actuators
# reset actuators to zero # 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) active = isActive(state)
for b in CS.buttonEvents: for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# button presses for rear view # button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker": if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed: 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 ***** # ***** state specific actions *****
# DISABLED # DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]: if state in [State.preEnabled, State.disabled]:
awareness_status = 1.
LaC.reset() LaC.reset()
LoC.reset(v_pid=CS.vEgo) LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING # ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]: elif state in [State.enabled, State.softDisabling]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on # decrease awareness status
awareness_status -= 0.01/(AWARENESS_TIME) awareness_status -= 0.01/(AWARENESS_TIME)
if awareness_status <= 0.: if awareness_status <= 0.:
AM.add("driverDistracted", enabled) AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ 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) AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface # 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: if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH 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 *** # *** process alerts ***
AM.process_alerts(sec_since_boot()) 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 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, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset, passive): 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.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.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2 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 dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process # 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.vEgo = CS.vEgo
dat.live100.vEgoRaw = CS.vEgoRaw dat.live100.vEgoRaw = CS.vEgoRaw
dat.live100.angleSteers = CS.steeringAngle dat.live100.angleSteers = CS.steeringAngle
dat.live100.curvature = VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo)
dat.live100.steerOverride = CS.steeringPressed dat.live100.steerOverride = CS.steeringPressed
# high level control state # 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): def controlsd_thread(gctx, rate=100):
# start the loop # start the loop
set_realtime_priority(2) set_realtime_priority(3)
context = zmq.Context() context = zmq.Context()
@ -429,9 +440,11 @@ def controlsd_thread(gctx, rate=100):
sendcan = None sendcan = None
# sub # sub
thermal = messaging.sub_sock(context, service_list['thermal'].port) poller = zmq.Poller()
health = messaging.sub_sock(context, service_list['health'].port) thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port) 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) logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message() CC = car.CarControl.new_message()
@ -439,9 +452,6 @@ def controlsd_thread(gctx, rate=100):
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
if CI is None: if CI is None:
if passive:
return
else:
raise Exception("unsupported car") raise Exception("unsupported car")
if passive: if passive:
@ -461,7 +471,7 @@ def controlsd_thread(gctx, rate=100):
# write CarParams # write CarParams
params.put("CarParams", CP.to_bytes()) params.put("CarParams", CP.to_bytes())
state = State.DISABLED state = State.disabled
soft_disable_timer = 0 soft_disable_timer = 0
v_cruise_kph = 255 v_cruise_kph = 255
overtemp = False overtemp = False
@ -472,6 +482,7 @@ def controlsd_thread(gctx, rate=100):
# 0.0 - 1.0 # 0.0 - 1.0
awareness_status = 1. awareness_status = 1.
v_cruise_kph_last = 0
rk = Ratekeeper(rate, print_delay_threshold=2./1000) rk = Ratekeeper(rate, print_delay_threshold=2./1000)
@ -485,14 +496,14 @@ def controlsd_thread(gctx, rate=100):
except (ValueError, KeyError): except (ValueError, KeyError):
pass pass
prof = Profiler() prof = Profiler(False) # off by default
while 1: while 1:
prof.reset() # avoid memory leak prof.checkpoint("Ratekeeper", ignore=True) # rk is here
# sample data and compute car events # 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) overtemp, free_space)
prof.checkpoint("Sample") prof.checkpoint("Sample")
@ -502,23 +513,25 @@ def controlsd_thread(gctx, rate=100):
if not passive: if not passive:
# update control state # 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") prof.checkpoint("State transition")
# compute actuators # compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, 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, v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM,
rear_view_allowed, rear_view_toggle) angle_offset, rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control") prof.checkpoint("State Control")
# publish data # 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, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent") prof.checkpoint("Sent")
# *** run loop at fixed rate *** # *** run loop at fixed rate ***
if rk.keep_time(): rk.keep_time()
prof.display() prof.display()

@ -1,19 +1,25 @@
from cereal import car from cereal import car, log
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from common.realtime import sec_since_boot
import copy import copy
# Priority # Priority
class PT: class Priority:
HIGH = 3 HIGH = 3
MID = 2 MID = 2
LOW = 1 LOW = 1
LOWEST = 0
AlertSize = log.Live100Data.AlertSize
AlertStatus = log.Live100Data.AlertStatus
class Alert(object): class Alert(object):
def __init__(self, def __init__(self,
alert_text_1, alert_text_1,
alert_text_2, alert_text_2,
alert_status,
alert_size,
alert_priority, alert_priority,
visual_alert, visual_alert,
audible_alert, audible_alert,
@ -23,6 +29,8 @@ class Alert(object):
self.alert_text_1 = alert_text_1 self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2 self.alert_text_2 = alert_text_2
self.alert_status = alert_status
self.alert_size = alert_size
self.alert_priority = alert_priority self.alert_priority = alert_priority
self.visual_alert = visual_alert if visual_alert is not None else "none" self.visual_alert = visual_alert if visual_alert is not None else "none"
self.audible_alert = audible_alert if audible_alert is not None else "none" self.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_hud_alert = duration_hud_alert
self.duration_text = duration_text self.duration_text = duration_text
self.start_time = 0.
# typecheck that enums are valid on startup # typecheck that enums are valid on startup
tst = car.CarControl.new_message() tst = car.CarControl.new_message()
tst.hudControl.visualAlert = self.visual_alert tst.hudControl.visualAlert = self.visual_alert
@ -51,347 +61,418 @@ class AlertManager(object):
"enable": Alert( "enable": Alert(
"", "",
"", "",
PT.MID, None, "beepSingle", .2, 0., 0.), AlertStatus.normal, AlertSize.none,
Priority.MID, None, "beepSingle", .2, 0., 0.),
"disable": Alert( "disable": Alert(
"", "",
"", "",
PT.MID, None, "beepSingle", .2, 0., 0.), AlertStatus.normal, AlertSize.none,
Priority.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert( "fcw": Alert(
"Brake", "Brake!",
"Risk of Collision", "Risk of Collision",
PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
"steerSaturated": Alert( "steerSaturated": Alert(
"Take Control", "Take Control",
"Turn Exceeds Limit", "Turn Exceeds Limit",
PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
"steerTempUnavailable": Alert( "steerTempUnavailable": Alert(
"Take Control", "Take Control",
"Steer Temporarily Unavailable", "Steer Temporarily Unavailable",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": Alert( "preDriverDistracted": Alert(
"Take Control", "Take Control",
"User Distracted", "User Distracted",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
"driverDistracted": Alert( "driverDistracted": Alert(
"Take Control to Regain Speed", "Take Control to Regain Speed",
"User Distracted", "User Distracted",
PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
"startup": Alert( "startup": Alert(
"Always Keep Hands on Wheel", "Always Keep Hands on Wheel",
"Be Ready to Take Over Any Time", "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( "ethicalDilemma": Alert(
"Take Control Immediately", "Take Control Immediately",
"Ethical Dilemma Detected", "Ethical Dilemma Detected",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailableNoEntry": Alert( "steerTempUnavailableNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Steer Temporary Unavailable", "Steer Temporary Unavailable",
PT.LOW, None, "chimeDouble", .4, 0., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"manualRestart": Alert( "manualRestart": Alert(
"Take Control", "Take Control",
"Resume Driving Manually", "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 # Non-entry only alerts
"wrongCarModeNoEntry": Alert( "wrongCarModeNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Main Switch Off", "Main Switch Off",
PT.LOW, None, "chimeDouble", .4, 0., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"dataNeededNoEntry": Alert( "dataNeededNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Data needed for calibration. Upload drive, try again", "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( "outOfSpaceNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Out of Space", "Out of Space",
PT.LOW, None, "chimeDouble", .4, 0., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"pedalPressedNoEntry": Alert( "pedalPressedNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Pedal Pressed", "Pedal Pressed",
PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
"speedTooLowNoEntry": Alert( "speedTooLowNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Speed Too Low", "Speed Too Low",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeHoldNoEntry": Alert( "brakeHoldNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Brake Hold Active", "Brake Hold Active",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"parkBrakeNoEntry": Alert( "parkBrakeNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Park Brake Engaged", "Park Brake Engaged",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"lowSpeedLockoutNoEntry": Alert( "lowSpeedLockoutNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Cruise Fault: Restart the Car", "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( "overheat": Alert(
"Take Control Immediately", "Take Control Immediately",
"System Overheated", "System Overheated",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": Alert( "wrongGear": Alert(
"Take Control Immediately", "Take Control Immediately",
"Gear not D", "Gear not D",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": Alert( "calibrationInvalid": Alert(
"Take Control Immediately", "Take Control Immediately",
"Calibration Invalid: Reposition Neo and Recalibrate", "Calibration Invalid: Reposition EON and Recalibrate",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": Alert( "calibrationInProgress": Alert(
"Take Control Immediately", "Take Control Immediately",
"Calibration in Progress", "Calibration in Progress",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": Alert( "doorOpen": Alert(
"Take Control Immediately", "Take Control Immediately",
"Door Open", "Door Open",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": Alert( "seatbeltNotLatched": Alert(
"Take Control Immediately", "Take Control Immediately",
"Seatbelt Unlatched", "Seatbelt Unlatched",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": Alert( "espDisabled": Alert(
"Take Control Immediately", "Take Control Immediately",
"ESP Off", "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( "radarCommIssue": Alert(
"Take Control Immediately", "Take Control Immediately",
"Radar Error: Restart the Car", "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( "radarFault": Alert(
"Take Control Immediately", "Take Control Immediately",
"Radar Error: Restart the Car", "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( "modelCommIssue": Alert(
"Take Control Immediately", "Take Control Immediately",
"Model Error: Restart the Car", "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( "controlsFailed": Alert(
"Take Control Immediately", "Take Control Immediately",
"Controls Failed", "Controls Failed",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"controlsMismatch": Alert( "controlsMismatch": Alert(
"Take Control Immediately", "Take Control Immediately",
"Controls Mismatch", "Controls Mismatch",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"commIssue": Alert( "commIssue": Alert(
"Take Control Immediately", "Take Control Immediately",
"CAN Error: Restart the Car", "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( "steerUnavailable": Alert(
"Take Control Immediately", "Take Control Immediately",
"Steer Error: Restart the Car", "Steer Fault: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"brakeUnavailable": Alert( "brakeUnavailable": Alert(
"Take Control Immediately", "Take Control Immediately",
"Brake Error: Restart the Car", "Brake Fault: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"gasUnavailable": Alert( "gasUnavailable": Alert(
"Take Control Immediately", "Take Control Immediately",
"Gas Error: Restart the Car", "Gas Fault: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"reverseGear": Alert( "reverseGear": Alert(
"Take Control Immediately", "Take Control Immediately",
"Reverse Gear", "Reverse Gear",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"cruiseDisabled": Alert( "cruiseDisabled": Alert(
"Take Control Immediately", "Take Control Immediately",
"Cruise Is Off", "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) # not loud cancellations (user is in control)
"noTarget": Alert( "noTarget": Alert(
"Comma Canceled", "Comma Canceled",
"No Close Lead", "No Close Lead",
PT.HIGH, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
"speedTooLow": Alert( "speedTooLow": Alert(
"Comma Canceled", "Comma Canceled",
"Speed Too Low", "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 # Cancellation alerts causing non-entry
"overheatNoEntry": Alert( "overheatNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"System Overheated", "System Overheated",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"wrongGearNoEntry": Alert( "wrongGearNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Gear not D", "Gear not D",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInvalidNoEntry": Alert( "calibrationInvalidNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Calibration Invalid: Reposition Neo and Recalibrate", "Calibration Invalid: Reposition EON and Recalibrate",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInProgressNoEntry": Alert( "calibrationInProgressNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Calibration in Progress", "Calibration in Progress",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"doorOpenNoEntry": Alert( "doorOpenNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Door Open", "Door Open",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"seatbeltNotLatchedNoEntry": Alert( "seatbeltNotLatchedNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Seatbelt Unlatched", "Seatbelt Unlatched",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"espDisabledNoEntry": Alert( "espDisabledNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"ESP Off", "ESP Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"radarCommIssueNoEntry": Alert( "radarCommIssueNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Radar Error: Restart the Car", "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( "radarFaultNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Radar Error: Restart the Car", "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( "modelCommIssueNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Model Error: Restart the Car", "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( "controlsFailedNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Controls Failed", "Controls Failed",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsMismatchNoEntry": Alert(
"Comma Unavailable",
"Controls Mismatch",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"commIssueNoEntry": Alert( "commIssueNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"CAN Error: Restart the Car", "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( "steerUnavailableNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Steer Error: Restart the Car", "Steer Fault: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeUnavailableNoEntry": Alert( "brakeUnavailableNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Brake Error: Restart the Car", "Brake Fault: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"gasUnavailableNoEntry": Alert( "gasUnavailableNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Gas Error: Restart the Car", "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( "reverseGearNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Reverse Gear", "Reverse Gear",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"cruiseDisabledNoEntry": Alert( "cruiseDisabledNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"Cruise is Off", "Cruise is Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.), AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"noTargetNoEntry": Alert( "noTargetNoEntry": Alert(
"Comma Unavailable", "Comma Unavailable",
"No Close Lead", "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): def __init__(self):
self.activealerts = [] self.activealerts = []
self.current_alert = None
def alertPresent(self): def alertPresent(self):
return len(self.activealerts) > 0 return len(self.activealerts) > 0
def add(self, alert_type, enabled=True, extra_text=''): def add(self, alert_type, enabled=True, extra_text=''):
alert_type = str(alert_type) alert_type = str(alert_type)
this_alert = copy.copy(self.alerts[alert_type]) added_alert = copy.copy(self.alerts[alert_type])
this_alert.alert_text_2 += extra_text added_alert.alert_text_2 += extra_text
added_alert.start_time = sec_since_boot()
# if new alert is higher priority, log it # 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', cloudlog.event('alert_add',
alert_type=alert_type, alert_type=alert_type,
enabled=enabled) enabled=enabled)
self.activealerts.append(this_alert) self.activealerts.append(added_alert)
self.activealerts.sort() self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True)
# TODO: cycle through alerts?
def process_alerts(self, cur_time): def process_alerts(self, cur_time):
if self.alertPresent():
self.alert_start_time = cur_time # first get rid of all the expired alerts
self.current_alert = self.activealerts[0] self.activealerts = [a for a in self.activealerts if a.start_time +
print self.current_alert 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 # start with assuming no alerts
self.alert_text_1 = "" self.alert_text_1 = ""
self.alert_text_2 = "" self.alert_text_2 = ""
self.alert_status = AlertStatus.normal
self.alert_size = AlertSize.none
self.visual_alert = "none" self.visual_alert = "none"
self.audible_alert = "none" self.audible_alert = "none"
if self.current_alert is not None: if ca:
# ewwwww if ca.start_time + ca.duration_sound > cur_time:
if self.alert_start_time + self.current_alert.duration_sound > cur_time: self.audible_alert = ca.audible_alert
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 self.alert_start_time + self.current_alert.duration_text > cur_time: if ca.start_time + ca.duration_hud_alert > cur_time:
self.alert_text_1 = self.current_alert.alert_text_1 self.visual_alert = ca.visual_alert
self.alert_text_2 = self.current_alert.alert_text_2
# disable current alert if ca.start_time + ca.duration_text > cur_time:
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.alert_text_1 = ca.alert_text_1
self.current_alert.duration_text) < cur_time: self.alert_text_2 = ca.alert_text_2
self.current_alert = None self.alert_status = ca.alert_status
self.alert_size = ca.alert_size
# reset
self.activealerts = []

@ -10,6 +10,7 @@ class EventTypes:
USER_DISABLE = 'userDisable' USER_DISABLE = 'userDisable'
SOFT_DISABLE = 'softDisable' SOFT_DISABLE = 'softDisable'
IMMEDIATE_DISABLE = 'immediateDisable' IMMEDIATE_DISABLE = 'immediateDisable'
PERMANENT = 'permanent'
def create_event(name, types): def create_event(name, types):

@ -8,7 +8,7 @@ from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
# 100ms is a rule of thumb estimation of lag from image processing to actuator command # 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 = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz _DT_MPC = 0.05 # 20Hz
@ -28,11 +28,11 @@ class LatControl(object):
def __init__(self, VM): def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) 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.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 = libmpc_py.libmpc
self.libmpc.init() self.libmpc.init(steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *") self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_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))) nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot() t = sec_since_boot()
if nans: if nans:
self.libmpc.init() self.libmpc.init(VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0: if t > self.last_cloudlog_t + 5.0:
@ -95,7 +95,10 @@ class LatControl(object):
self.pid.reset() self.pid.reset()
else: 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 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) steers_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steers_max self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max self.pid.neg_limit = -steers_max

@ -72,14 +72,12 @@ int main( )
// Angular rate error // Angular rate error
h << (v_ref + 1.0 ) * t; h << (v_ref + 1.0 ) * t;
DMatrix Q(5,5); BMatrix Q(5,5); Q.setAll(true);
Q(0,0) = 1.0; // Q(0,0) = 1.0;
Q(1,1) = 1.0; // Q(1,1) = 1.0;
Q(2,2) = 1.0; // Q(2,2) = 1.0;
// Q(3,3) = 1.0;
Q(3,3) = 1.0; // Q(4,4) = 2.0;
Q(4,4) = 1.0;
// Terminal cost // Terminal cost
Function hN; Function hN;
@ -92,18 +90,27 @@ int main( )
// Heading errors // Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle - psi); hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
DMatrix QN(4,4); BMatrix QN(4,4); QN.setAll(true);
QN(0,0) = 1.0; // QN(0,0) = 1.0;
QN(1,1) = 1.0; // QN(1,1) = 1.0;
QN(2,2) = 1.0; // QN(2,2) = 1.0;
// QN(3,3) = 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 // Setup Optimal Control Problem
const double tStart = 0.0; 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.subjectTo(f);
ocp.minimizeLSQ(Q, h); ocp.minimizeLSQ(Q, h);
@ -120,6 +127,7 @@ int main( )
mpc.set( INTEGRATOR_TYPE, INT_RK4 ); mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
mpc.set( MAX_NUM_QP_ITERATIONS, 500); mpc.set( MAX_NUM_QP_ITERATIONS, 500);
mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES ); mpc.set( QP_SOLVER, QP_QPOASES );

@ -1,11 +1,12 @@
import os import os
import sys
import subprocess import subprocess
from cffi import FFI from cffi import FFI
mpc_dir = os.path.dirname(os.path.abspath(__file__)) mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so") 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 = FFI()
ffi.cdef(""" ffi.cdef("""
@ -14,13 +15,13 @@ typedef struct {
} state_t; } state_t;
typedef struct { typedef struct {
double x[50]; double x[20];
double y[50]; double y[20];
double psi[50]; double psi[20];
double delta[50]; double delta[20];
} log_t; } log_t;
void init(); void init(double steer_rate_cost);
int run_mpc(state_t * x0, log_t * solution, int run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4], 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); double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);

@ -28,7 +28,7 @@ typedef struct {
double delta[N]; double delta[N];
} log_t; } log_t;
void init(){ void init(double steerRateCost){
acado_initializeSolver(); acado_initializeSolver();
int i; int i;
@ -42,6 +42,22 @@ void init(){
/* MPC: initialize the current state feedback. */ /* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; 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, int run_mpc(state_t * x0, log_t * solution,

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:170140586eb60aa9e772acd0988eb8f5df3549da1a2de1539aff768d940346e2 oid sha256:6f0966261166f2380ad7e37389060bd4011428e8f0ef0edaa6f2d439dc1de1e6
size 8680 size 8823

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:e02be34d0d234957aa4a69673c4e9bb6584572f2e62b969d9c3281bdf7daabb4 oid sha256:5cf12c96cffb69f1e659a20760c9ad3ab74ecce3d88aba1e50af359ab14c88da
size 18490 size 18662

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1 oid sha256:77977740e5e95a7a0e86ec4cc903a09fa528934d1221f7100499176006b6b8fd
size 1949 size 1948

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:75a219c9aaacbbb4470b1be437005736667e9a190a6467e6d254201c2fe26b09 oid sha256:a5f24abe53c09556bfd27179c9255ce4674d88c38e6554d10e99b60ddd10d0c5
size 1822 size 1821

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20 oid sha256:2fe4faa6b70f8cb9fe000412093579a5bf96890831d62c83d01deff94cdb8d19
size 194688 size 393169

@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.pid import PIController
STOPPING_EGO_SPEED = 0.5 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 STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
STARTING_TARGET_SPEED = 0.5 STARTING_TARGET_SPEED = 0.5
BRAKE_THRESHOLD_TO_PID = 0.2 BRAKE_THRESHOLD_TO_PID = 0.2
@ -101,7 +101,7 @@ class LongControl(object):
elif self.long_control_state == LongCtrlState.pid: elif self.long_control_state == LongCtrlState.pid:
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 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.pos_limit = gas_max
self.pid.neg_limit = - brake_max self.pid.neg_limit = - brake_max
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)

@ -44,11 +44,8 @@ int main( )
h << a_ego * (1.0 + v_ego / 10.0); h << a_ego * (1.0 + v_ego / 10.0);
h << j_ego * (1.0 + v_ego / 10.0); h << j_ego * (1.0 + v_ego / 10.0);
DMatrix Q(4,4); // Weights are defined in mpc.
Q(0,0) = 5.0; BMatrix Q(4,4); Q.setAll(true);
Q(1,1) = 0.1;
Q(2,2) = 10.0;
Q(3,3) = 20.0;
// Terminal cost // Terminal cost
Function hN; Function hN;
@ -56,16 +53,24 @@ int main( )
hN << (d_l - desired) / (0.1 * v_ego + 0.5); hN << (d_l - desired) / (0.1 * v_ego + 0.5);
hN << a_ego * (1.0 + v_ego / 10.0); hN << a_ego * (1.0 + v_ego / 10.0);
DMatrix QN(3,3); // Weights are defined in mpc.
QN(0,0) = 5.0; BMatrix QN(3,3); QN.setAll(true);
QN(1,1) = 0.1;
QN(2,2) = 10.0; // 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 // Setup Optimal Control Problem
const double tStart = 0.0; 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.subjectTo(f);
ocp.minimizeLSQ(Q, h); ocp.minimizeLSQ(Q, h);
@ -78,8 +83,9 @@ int main( )
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 ); 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( MAX_NUM_QP_ITERATIONS, 500);
mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES ); mpc.set( QP_SOLVER, QP_QPOASES );

@ -1,10 +1,11 @@
import os import os
import sys
import subprocess import subprocess
from cffi import FFI from cffi import FFI
mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) 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): def _get_libmpc(mpc_id):
@ -18,13 +19,13 @@ def _get_libmpc(mpc_id):
typedef struct { typedef struct {
double x_ego[50]; double x_ego[20];
double v_ego[50]; double v_ego[20];
double a_ego[50]; double a_ego[20];
double j_ego[50]; double j_ego[20];
double x_l[50]; double x_l[20];
double v_l[50]; double v_l[20];
double a_l[50]; double a_l[20];
} log_t; } log_t;
void init(); void init();

@ -45,6 +45,22 @@ void init(){
/* MPC: initialize the current state feedback. */ /* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; 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){ 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; double dt = 0.2;
for (i = 0; i < N + 1; ++i){ for (i = 0; i < N + 1; ++i){
if (i > 4){
dt = 0.6;
}
acadoVariables.x[i*NX] = x_ego; acadoVariables.x[i*NX] = x_ego;
acadoVariables.x[i*NX+1] = v_ego; acadoVariables.x[i*NX+1] = v_ego;
acadoVariables.x[i*NX+2] = a_ego; acadoVariables.x[i*NX+2] = a_ego;

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:6db0b40b4f066266c4382512de1752cd8fd2260bcd58355f1291e5db272b7ec1 oid sha256:3cc46e29bac93957764e9218e7755e1986bdb9c532585371c67f4a0c544a6c04
size 8655 size 8760

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:fbe29a0acd4c2ec8fea880b159440ac68b8d0b6ab7437c95c6f84443db13abef oid sha256:225206dbed64d7d842a9f2390a6f3d8ebbfa2e5fbc2290f44ff5ee716faff2d4
size 33237 size 33409

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:ac20ce29802179e0d9b91f2a43673e6a0a41c2d1abcecadd54daca7a08befda8 oid sha256:7aaa8e6766705d1cd22c65d418e10a4c4b54864809cc9ad5e5de09c98e72cfd0
size 1948 size 1948

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:e1ccfb2ae276bc3bc787e2bfc68861de08017e8ff97411fe5ceab65ae9997f18 oid sha256:383c2fa9974e0802dd86773aed20e068b39e0e5c505e6a641b30332f99f875b7
size 1821 size 1821

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:5d9df24f3df44e3fbd8d4d9695c8f3f452e391179f01ba8c52d28473e1bbd6a3 oid sha256:5883cfa43628f11c0e4d49ce216684cbca29070640f1c3ab8801ab5ad0d4334a
size 303043 size 447794

@ -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.pathplanner import PathPlanner
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
from selfdrive.controls.lib.speed_smoother import speed_smoother 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 = 0.01 # 100Hz
_DT_MPC = 0.2 # 5Hz _DT_MPC = 0.2 # 5Hz
@ -255,8 +255,9 @@ class Planner(object):
def __init__(self, CP, fcw_enabled): def __init__(self, CP, fcw_enabled):
context = zmq.Context() context = zmq.Context()
self.CP = CP self.CP = CP
self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.poller = zmq.Poller()
self.model = messaging.sub_sock(context, service_list['model'].port) 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.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].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() cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS 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: if md is not None:
self.last_md_ts = md.logMonoTime self.last_md_ts = md.logMonoTime
self.last_model = cur_time self.last_model = cur_time
@ -334,7 +343,6 @@ class Planner(object):
self.PP.update(CS.vEgo, md) self.PP.update(CS.vEgo, md)
l20 = messaging.recv_sock(self.live20) if md is None else None
if l20 is not None: if l20 is not None:
self.last_l20_ts = l20.logMonoTime self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time self.last_l20 = cur_time
@ -368,17 +376,21 @@ class Planner(object):
accel_limits[1], accel_limits[0], accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0], jerk_limits[1], jerk_limits[0],
_DT_MPC) _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else: else:
starting = LoC.long_control_state == LongCtrlState.starting starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0) a_ego = min(CS.aEgo, 0.0)
self.v_cruise = CS.vEgo reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
self.a_cruise = self.CP.startAccel if starting else a_ego reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc_start = CS.vEgo self.v_acc = reset_speed
self.a_acc_start = self.CP.startAccel if starting else a_ego self.a_acc = reset_accel
self.v_acc = CS.vEgo self.v_acc_start = reset_speed
self.a_acc = self.CP.startAccel if starting else a_ego self.a_acc_start = reset_accel
self.v_acc_sol = CS.vEgo self.v_cruise = reset_speed
self.a_acc_sol = self.CP.startAccel if starting else a_ego 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.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) 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) \ self.lead_1.fcw, blinkers) \
and not CS.brakePressed and not CS.brakePressed
if self.fcw: if self.fcw:
cloudlog.info("FCW triggered") cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5: if cur_time - self.last_model > 0.5:
self.model_dead = True self.model_dead = True

@ -67,11 +67,16 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
if aPeak > aMax: if aPeak > aMax:
aPeak = aMax aPeak = aMax
t1 = (aPeak - aEgo) / jMax t1 = (aPeak - aEgo) / jMax
if aPeak <= 0: # there is no solution, so stop after t1
t2 = t1 + ts + 1e-9
t3 = t2
else:
vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
if vChange < aPeak * ts: if vChange < aPeak * ts:
t2 = t1 + vChange / aPeak t2 = t1 + vChange / aPeak
else: else:
t2 = t1 + ts t2 = t1 + ts
t3 = t2 - aPeak / jMin
else: else:
t1 = (aPeak - aEgo) / jMax t1 = (aPeak - aEgo) / jMax
t2 = t1 t2 = t1

@ -89,6 +89,8 @@ class VehicleModel(object):
A, B = create_dyn_state_matrices(u, self) A, B = create_dyn_state_matrices(u, self)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt 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__': if __name__ == '__main__':
from selfdrive.car.honda.interface import CarInterface from selfdrive.car.honda.interface import CarInterface

@ -44,11 +44,12 @@ class EKFV1D(EKF):
# fuses camera and radar data for best lead detection # fuses camera and radar data for best lead detection
def radard_thread(gctx=None): def radard_thread(gctx=None):
set_realtime_priority(1) set_realtime_priority(2)
# wait for stats about the car to come in from controls # wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams") cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked= CP.radarName == "mock"
VM = VehicleModel(CP) VM = VehicleModel(CP)
cloudlog.info("radard got CarParams") cloudlog.info("radard got CarParams")
@ -59,8 +60,9 @@ def radard_thread(gctx=None):
context = zmq.Context() context = zmq.Context()
# *** subscribe to features and model from visiond # *** subscribe to features and model from visiond
model = messaging.sub_sock(context, service_list['model'].port) poller = zmq.Poller()
live100 = messaging.sub_sock(context, service_list['live100'].port) 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() PP = PathPlanner()
RI = RadarInterface() RI = RadarInterface()
@ -96,7 +98,6 @@ def radard_thread(gctx=None):
rk = Ratekeeper(rate, print_delay_threshold=np.inf) rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1: while 1:
rr = RI.update() rr = RI.update()
ar_pts = {} 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] ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s # 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: if l100 is not None:
active = l100.live100.active active = l100.live100.active
v_ego = l100.live100.vEgo v_ego = l100.live100.vEgo
@ -119,7 +128,6 @@ def radard_thread(gctx=None):
if v_ego is None: if v_ego is None:
continue continue
md = messaging.recv_sock(model)
if md is not None: if md is not None:
last_md_ts = md.logMonoTime last_md_ts = md.logMonoTime
@ -140,9 +148,11 @@ def radard_thread(gctx=None):
del ar_pts[VISION_POINT] del ar_pts[VISION_POINT]
# *** compute the likely path_y *** # *** 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) 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] path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data *** # *** remove missing points from meta data ***
@ -152,8 +162,8 @@ def radard_thread(gctx=None):
# *** compute the tracks *** # *** compute the tracks ***
for ids in ar_pts: for ids in ar_pts:
# ignore the vision point for now # ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT: if ids == VISION_POINT and not mocked:
continue continue
rpt = ar_pts[ids] rpt = ar_pts[ids]
@ -185,33 +195,11 @@ def radard_thread(gctx=None):
tracks[fused_id].vision_cnt += 1 tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion() tracks[fused_id].update_vision_fusion()
# publish tracks (debugging)
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
if DEBUG: if DEBUG:
print "NEW CYCLE" print "NEW CYCLE"
if VISION_POINT in ar_pts: if VISION_POINT in ar_pts:
print "vision", ar_pts[VISION_POINT] 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() idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) 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. dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes()) 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() rk.monitor_time()
def main(gctx=None): def main(gctx=None):

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:b7988b5c9f39ef6650f25e1a8181d4eb2cfa1ded0a89d871440f121f0234f02e oid sha256:0fdb0771564e2b6624375879ce5a937dd7736dbd4717c729b6265197237e37a3
size 1412368 size 1412368

@ -9,6 +9,7 @@ import inspect
import requests import requests
import traceback import traceback
import threading import threading
import subprocess
from collections import Counter from collections import Counter
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
@ -66,6 +67,13 @@ def clear_locks(root):
except OSError: except OSError:
cloudlog.exception("clear_locks failed") 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): class Uploader(object):
def __init__(self, dongle_id, access_token, root): def __init__(self, dongle_id, access_token, root):
@ -111,12 +119,13 @@ class Uploader(object):
total_size += os.stat(fn).st_size total_size += os.stat(fn).st_size
return dict(name_counts), total_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 # try to upload log files first
for name, key, fn in self.gen_upload_files(): for name, key, fn in self.gen_upload_files():
if name in ["rlog", "rlog.bz2"]: if name in ["rlog", "rlog.bz2"]:
return (key, fn, 0) return (key, fn, 0)
if with_video:
# then upload compressed camera file # then upload compressed camera file
for name, key, fn in self.gen_upload_files(): for name, key, fn in self.gen_upload_files():
if name in ["fcamera.hevc"]: if name in ["fcamera.hevc"]:
@ -240,13 +249,16 @@ def uploader_fn(exit_event):
uploader = Uploader(dongle_id, access_token, ROOT) uploader = Uploader(dongle_id, access_token, ROOT)
while True: while True:
upload_video = (params.get("IsUploadVideoOverCellularEnabled") != "0") or is_on_wifi()
backoff = 0.1 backoff = 0.1
while True: while True:
if exit_event.is_set(): if exit_event.is_set():
return return
d = uploader.next_file_to_upload() d = uploader.next_file_to_upload(upload_video)
if d is None: if d is None:
break break

@ -7,13 +7,9 @@ import errno
import signal import signal
if __name__ == "__main__": if __name__ == "__main__":
if os.path.isfile("/init.qcom.rc"): if os.path.isfile("/init.qcom.rc") \
# check if NEOS update is required and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 4):
while ((not os.path.isfile("/VERSION") raise Exception("NEOS outdated")
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)
# get a non-blocking stdout # get a non-blocking stdout
child_pid, child_pty = os.forkpty() child_pid, child_pty = os.forkpty()
@ -44,6 +40,7 @@ if __name__ == "__main__":
os._exit(os.wait()[1]) os._exit(os.wait()[1])
import shutil
import hashlib import hashlib
import importlib import importlib
import subprocess import subprocess
@ -219,22 +216,13 @@ def kill_managed_process(name):
def cleanup_all_processes(signal, frame): def cleanup_all_processes(signal, frame):
cloudlog.info("caught ctrl-c %s %s" % (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(): for name in running.keys():
kill_managed_process(name) kill_managed_process(name)
sys.exit(0) cloudlog.info("everything is dead")
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
# ****************** run loop ****************** # ****************** run loop ******************
@ -252,7 +240,11 @@ def manager_init():
cloudlog.info("dongle id is " + dongle_id) cloudlog.info("dongle id is " + dongle_id)
os.environ['DONGLE_ID'] = 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) cloudlog.info("dirty is %d" % dirty)
if not dirty: if not dirty:
os.environ['CLEAN'] = '1' os.environ['CLEAN'] = '1'
@ -280,12 +272,9 @@ def system(cmd):
output=e.output[-1024:], output=e.output[-1024:],
returncode=e.returncode) returncode=e.returncode)
# TODO: this is not proper gating for EON EON = os.path.exists("/EON")
try: if EON:
from smbus2 import SMBus from smbus2 import SMBus
EON = True
except ImportError:
EON = False
def setup_eon_fan(): def setup_eon_fan():
if not EON: if not EON:
@ -322,8 +311,10 @@ _TEMP_THRS_H = [50., 65., 80., 10000]
_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] _TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
# fan speed options # fan speed options
_FAN_SPEEDS = [0, 16384, 32768, 65535] _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_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) 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 # update speed if using the low thresholds results in fan speed decrement
fan_speed = new_speed_l 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) set_eon_fan(fan_speed/16384)
return fan_speed return fan_speed
@ -369,8 +364,6 @@ class LocationStarter(object):
return location.speed*3.6 > 10 return location.speed*3.6 > 10
def manager_thread(): def manager_thread():
global baseui_running
# now loop # now loop
context = zmq.Context() context = zmq.Context()
thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
@ -383,7 +376,8 @@ def manager_thread():
for p in persistent_processes: for p in persistent_processes:
start_managed_process(p) start_managed_process(p)
manage_baseui(True) # start frame
system("am start -n ai.comma.plus.frame/.MainActivity")
# do this before panda flashing # do this before panda flashing
setup_eon_fan() setup_eon_fan()
@ -391,7 +385,9 @@ def manager_thread():
if os.getenv("NOBOARD") is None: if os.getenv("NOBOARD") is None:
start_managed_process("pandad") start_managed_process("pandad")
passive = bool(os.getenv("PASSIVE")) params = Params()
passive = params.get("Passive") == "1"
passive_starter = LocationStarter() passive_starter = LocationStarter()
started_ts = None started_ts = None
@ -399,6 +395,7 @@ def manager_thread():
count = 0 count = 0
fan_speed = 0 fan_speed = 0
ignition_seen = False ignition_seen = False
battery_was_high = False
health_sock.RCVTIMEO = 1500 health_sock.RCVTIMEO = 1500
@ -424,13 +421,19 @@ def manager_thread():
msg.thermal.batteryPercent = int(f.read()) msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f: with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip() 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 # TODO: add car battery voltage check
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 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.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()) thermal_sock.send(msg.to_bytes())
print msg print msg
@ -448,8 +451,10 @@ def manager_thread():
ignition = td is not None and td.health.started ignition = td is not None and td.health.started
ignition_seen = ignition_seen or ignition ignition_seen = ignition_seen or ignition
params = Params() do_uninstall = params.get("DoUninstall") == "1"
should_start = ignition and (params.get("HasAcceptedTerms") == "1") accepted_terms = params.get("HasAcceptedTerms") == "1"
should_start = ignition
# start on gps in passive mode # start on gps in passive mode
if passive and not ignition_seen: if passive and not ignition_seen:
@ -458,6 +463,15 @@ def manager_thread():
# with 2% left, we killall, otherwise the phone is bricked # with 2% left, we killall, otherwise the phone is bricked
should_start = should_start and avail > 0.02 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 should_start:
if not started_ts: if not started_ts:
@ -468,20 +482,17 @@ def manager_thread():
kill_managed_process(p) kill_managed_process(p)
else: else:
start_managed_process(p) start_managed_process(p)
manage_baseui(False)
else: else:
manage_baseui(True)
started_ts = None started_ts = None
logger_dead = False logger_dead = False
for p in car_started_processes: for p in car_started_processes:
kill_managed_process(p) kill_managed_process(p)
# shutdown if the battery gets lower than 10%, we aren't running, and we are 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": if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high:
os.system('LD_LIBRARY_PATH="" svc power shutdown') os.system('LD_LIBRARY_PATH="" svc power shutdown')
if msg.thermal.batteryPercent > 10:
# check the status of baseui battery_was_high = True
baseui_running = 'com.baseui' in subprocess.check_output(["ps"])
# check the status of all processes, did any of them die? # check the status of all processes, did any of them die?
for p in running: for p in running:
@ -495,10 +506,13 @@ def manager_thread():
health=(td.to_dict() if td else None), health=(td.to_dict() if td else None),
thermal=msg.to_dict()) thermal=msg.to_dict())
if do_uninstall:
break
count += 1 count += 1
def get_installed_apks(): 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 = {} ret = {}
for x in dat: for x in dat:
if x.startswith("package:"): if x.startswith("package:"):
@ -506,21 +520,16 @@ def get_installed_apks():
ret[k] = v ret[k] = v
return ret return ret
# optional, build the c++ binaries and preimport the python for speed def install_apk(path):
def manager_prepare(): # can only install from world readable path
if os.path.exists(os.path.join(BASEDIR, ".gitmodules")): install_path = "/sdcard/%s" % os.path.basename(path)
# update submodules shutil.copyfile(path, install_path)
system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
# build cereal first
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
# build all processes ret = subprocess.call(["pm", "install", "-r", install_path])
os.chdir(os.path.dirname(os.path.abspath(__file__))) os.remove(install_path)
for p in managed_processes: return ret == 0
prepare_managed_process(p)
def update_apks():
# install apks # install apks
installed = get_installed_apks() installed = get_installed_apks()
for app in os.listdir(os.path.join(BASEDIR, "apk/")): for app in os.listdir(os.path.join(BASEDIR, "apk/")):
@ -529,7 +538,8 @@ def manager_prepare():
if app not in installed: if app not in installed:
installed[app] = None installed[app] = None
cloudlog.info("installed apks %s" % (str(installed), )) 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") apk_path = os.path.join(BASEDIR, "apk/"+app+".apk")
if os.path.isfile(apk_path): if os.path.isfile(apk_path):
h1 = hashlib.sha1(open(apk_path).read()).hexdigest() h1 = hashlib.sha1(open(apk_path).read()).hexdigest()
@ -537,16 +547,37 @@ def manager_prepare():
if installed[app] is not None: if installed[app] is not None:
h2 = hashlib.sha1(open(installed[app]).read()).hexdigest() h2 = hashlib.sha1(open(installed[app]).read()).hexdigest()
cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2))
if h2 is None or h1 != h2: if h2 is None or h1 != h2:
cloudlog.info("installing %s" % app) cloudlog.info("installing %s" % app)
for do_uninstall in [False, True]:
if do_uninstall: success = install_apk(apk_path)
if not success:
cloudlog.info("needing to uninstall %s" % app) cloudlog.info("needing to uninstall %s" % app)
os.system("pm uninstall %s" % app) 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)) success = install_apk(apk_path)
if ret == 0:
break assert success
assert ret == 0
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(): def main():
if os.getenv("NOLOG") is not None: if os.getenv("NOLOG") is not None:
@ -581,11 +612,13 @@ def main():
if params.get("IsMetric") is None: if params.get("IsMetric") is None:
params.put("IsMetric", "0") params.put("IsMetric", "0")
if params.get("IsRearViewMirror") is None: if params.get("IsRearViewMirror") is None:
params.put("IsRearViewMirror", "1") params.put("IsRearViewMirror", "0")
if params.get("IsFcwEnabled") is None: if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1") params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None: if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0") params.put("HasAcceptedTerms", "0")
if params.get("IsUploadVideoOverCellularEnabled") is None:
params.put("IsUploadVideoOverCellularEnabled", "1")
params.put("Passive", "1" if os.getenv("PASSIVE") else "0") params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
@ -597,6 +630,7 @@ def main():
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True) close_fds=True)
try: try:
manager_update()
manager_init() manager_init()
manager_prepare() manager_prepare()
finally: finally:
@ -606,6 +640,9 @@ def main():
if os.getenv("PREPAREONLY") is not None: if os.getenv("PREPAREONLY") is not None:
return return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try: try:
manager_thread() manager_thread()
except Exception: except Exception:
@ -614,6 +651,9 @@ def main():
finally: finally:
cleanup_all_processes(None, None) cleanup_all_processes(None, None)
if params.get("DoUninstall") == "1":
uninstall()
if __name__ == "__main__": if __name__ == "__main__":
main() main()
# manual exit because we are forked # manual exit because we are forked

@ -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

@ -33,7 +33,6 @@ class RadarInterface(object):
self.radar_fault = False self.radar_fault = False
self.delay = 0.1 # Delay of radar self.delay = 0.1 # Delay of radar
self.cutin_prediction = True
# Nidec # Nidec
self.rcp = _create_nidec_can_parser() self.rcp = _create_nidec_can_parser()

@ -34,7 +34,6 @@ class RadarInterface(object):
# Nidec # Nidec
self.rcp = _create_radard_can_parser() self.rcp = _create_radard_can_parser()
self.cutin_prediction = False
context = zmq.Context() context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port) self.logcan = messaging.sub_sock(context, service_list['can'].port)

@ -19,16 +19,23 @@ def get_git_commit():
def get_git_branch(): def get_git_branch():
return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() 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(): def register():
params = Params() params = Params()
try:
params.put("Version", version) params.put("Version", version)
params.put("GitCommit", get_git_commit()) params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch()) 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: 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) dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii') dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii')

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:15804c73a05556fcbb976a266e66d4c22ec6f7dd4a98aeff89f15437252bdb3c oid sha256:c3aae2d2dbdb681bbaae67abcd6363bf0849d20907fba98e3d47e0bec4df14d1
size 981400 size 981416

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:51c5ed132e3984edfc2fbfd856169ea3d999cada499ef90ceba0122a35f857bb oid sha256:b6fead09853341320f8598840c06e0a125d5203336fa1ca6905e78a844b69709
size 972296 size 972296

@ -46,6 +46,8 @@ ubloxGnss: [8033, true]
clocks: [8034, true] clocks: [8034, true]
liveMpc: [8035, true] liveMpc: [8035, true]
liveLongitudinalMpc: [8036, true] liveLongitudinalMpc: [8036, true]
plusFrame: [8037, false]
navStatus: [8038, true]
testModel: [8040, false] testModel: [8040, false]

@ -60,6 +60,7 @@ class Maneuver(object):
gas=gas, brake=brake, steer_torque=steer_torque, gas=gas, brake=brake, steer_torque=steer_torque,
distance=distance, speed=speed, acceleration=acceleration, distance=distance, speed=speed, acceleration=acceleration,
up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd, 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, d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid, v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
cruise_speed=last_live100.vCruise, cruise_speed=last_live100.vCruise,

@ -20,6 +20,7 @@ class ManeuverPlot(object):
self.up_accel_cmd_array = [] self.up_accel_cmd_array = []
self.ui_accel_cmd_array = [] self.ui_accel_cmd_array = []
self.uf_accel_cmd_array = []
self.d_rel_array = [] self.d_rel_array = []
self.v_rel_array = [] self.v_rel_array = []
@ -38,8 +39,8 @@ class ManeuverPlot(object):
self.title = title self.title = title
def add_data(self, time, gas, brake, steer_torque, distance, speed, def add_data(self, time, gas, brake, steer_torque, distance, speed,
acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel,
v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
self.time_array.append(time) self.time_array.append(time)
self.gas_array.append(gas) self.gas_array.append(gas)
self.brake_array.append(brake) self.brake_array.append(brake)
@ -49,6 +50,7 @@ class ManeuverPlot(object):
self.acceleration_array.append(acceleration) self.acceleration_array.append(acceleration)
self.up_accel_cmd_array.append(up_accel_cmd) self.up_accel_cmd_array.append(up_accel_cmd)
self.ui_accel_cmd_array.append(ui_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.d_rel_array.append(d_rel)
self.v_rel_array.append(v_rel) self.v_rel_array.append(v_rel)
self.v_lead_array.append(v_lead) self.v_lead_array.append(v_lead)
@ -117,12 +119,13 @@ class ManeuverPlot(object):
plt.figure(plt_num) plt.figure(plt_num)
plt.plot( plt.plot(
np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g', 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.xlabel("Time, [s]")
plt.ylabel("Accel Cmd [m/s^2]") plt.ylabel("Accel Cmd [m/s^2]")
plt.grid() 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) pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
# relative distances chart ======= # relative distances chart =======

@ -4,7 +4,6 @@ os.environ['FAKEUPLOAD'] = "1"
from common.testing import phone_only from common.testing import phone_only
from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import manager_init, manager_prepare
from selfdrive.manager import start_managed_process, kill_managed_process, get_running from selfdrive.manager import start_managed_process, kill_managed_process, get_running
from selfdrive.manager import manage_baseui
from selfdrive.config import CruiseButtons from selfdrive.config import CruiseButtons
from functools import wraps from functools import wraps
import time import time
@ -93,10 +92,3 @@ def test_ui():
def test_uploader(): def test_uploader():
print "UPLOADER" print "UPLOADER"
time.sleep(10.0) time.sleep(10.0)
@phone_only
def test_baseui():
manage_baseui(True)
time.sleep(10.0)
manage_baseui(False)

@ -52,6 +52,7 @@ ui: $(OBJS)
$(CEREAL_LIBS) \ $(CEREAL_LIBS) \
$(ZMQ_LIBS) \ $(ZMQ_LIBS) \
-L/system/vendor/lib64 \ -L/system/vendor/lib64 \
-lhardware \
$(OPENGL_LIBS) \ $(OPENGL_LIBS) \
-lcutils -lm -llog -lcutils -lm -llog

File diff suppressed because it is too large Load Diff

@ -7,9 +7,6 @@ import time
import subprocess import subprocess
def main(gctx=None): def main(gctx=None):
if not os.getenv("CLEAN"):
return
while True: while True:
# try network # try network
r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:008834e55cbf9b93d3bc017a2aa56ac1f431e31bc4fc8a5f904642f757245c5e oid sha256:06b2290dd088740e5ba460f1d5bff98187530c6bf696518766ad58b9d1aa90ab
size 13334480 size 13337952

Loading…
Cancel
Save