diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..2357509e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +# How to contribute + +Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. + +Most open source development activity is coordinated through our [slack](https://slack.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/) + +## Getting Started + + * Join our slack [slack.comma.ai](https://slack.comma.ai) + * Make sure you have a [GitHub account](https://github.com/signup/free) + * Fork the repository on GitHub + +## Car Ports (openpilot) + +We've released a guide for porting to Toyota cars [here](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) + +If you port openpilot to a substantially new car, you might be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html) + diff --git a/LICENSE.openpilot b/LICENSE similarity index 96% rename from LICENSE.openpilot rename to LICENSE index 8a6c6976..7dafa944 100644 --- a/LICENSE.openpilot +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2016, Comma.ai, Inc. +Copyright (c) 2018, Comma.ai, Inc. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: diff --git a/README_chffrplus.md b/README_chffrplus.md new file mode 100644 index 00000000..dc13e8c3 --- /dev/null +++ b/README_chffrplus.md @@ -0,0 +1,36 @@ +Welcome to chffrplus +====== + +[chffrplus](https://github.com/commaai/chffrplus) is an open source dashcam. + +This is the shipping reference software for the comma EON Dashcam DevKit. It keeps many of the niceities of [openpilot](https://github.com/commaai/openpilot), like high quality sensors, great camera, and good autostart and stop. Though unlike openpilot, it cannot control your car. chffrplus can interface with your car through a [panda](https://shop.comma.ai/products/panda-obd-ii-dongle), but just like our dashcam app [chffr](https://getchffr.com/), it is read only. + +It integrates with the rest of the comma ecosystem, so you can view your drives on the [chffr](https://getchffr.com/) app for Android or iOS, and reverse engineer your car with [cabana](https://community.comma.ai/cabana/?demo=1). + + +Hardware +------ + +Right now chffrplus supports the [EON Dashcam DevKit](https://shop.comma.ai/products/eon-dashcam-devkit) for hardware to run on. + +Install chffrplus on a EON device by entering ``https://chffrplus.comma.ai`` during NEOS setup. + + +User Data / chffr Account / Crash Reporting +------ + +By default chffrplus creates an account and includes a client for chffr, our dashcam app. + +It's open source software, so you are free to disable it if you wish. + +It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. +It does not log the user facing camera or the microphone. + +By using it, you agree to [our privacy policy](https://beta.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data. + + +Licensing +------ + +chffrplus is released under the MIT license. + diff --git a/RELEASES.md b/RELEASES.md index 1acd4fc8..f0280d43 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,15 @@ +Version 0.4.3 (2018-03-13) +========================== + * Add HDR and autofocus + * Update UI aesthetic + * Grey panda works in Waze + * Add alpha support for 2017 Honda Pilot + * Slight increase in acceleration response from stop + * Switch CAN sending to use CANPacker + * Fix pulsing acceleration regression on Honda + * Fix openpilot bugs when stock system is in use + * Change starting logic for chffrplus to use battery voltage + Version 0.4.2 (2018-02-05) ========================== * Add alpha support for 2017 Lexus RX Hybrid diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 06fcf2a5..dc49d8f3 100644 Binary files a/apk/ai.comma.plus.frame.apk and b/apk/ai.comma.plus.frame.apk differ diff --git a/apk/external/com.waze.apkpatch b/apk/external/com.waze.apkpatch index 799c1023..31e86c56 100644 Binary files a/apk/external/com.waze.apkpatch and b/apk/external/com.waze.apkpatch differ diff --git a/apk/external/patcher.py b/apk/external/patcher.py index bb5e6e71..2666e5ee 100755 --- a/apk/external/patcher.py +++ b/apk/external/patcher.py @@ -24,7 +24,7 @@ APKS = { 'src': 'https://apkcache.s3.amazonaws.com/com.waze_1021278.apk', 'src_sha256': 'f00957e93e2389f9e30502ac54994b98ac769314b0963c263d4e8baa625ab0c2', 'patch': 'com.waze.apkpatch', - 'out_sha256': '9ec8b0ea3c78c666342865b1bfb66e368a3f5c911df2ad12835206ec8b19f444' + 'out_sha256': 'fee880a91a44c738442cd05fd1b6d9b5817cbf755aa61c86325ada2bc443d5cf' }, 'com.spotify.music': { 'src': 'https://apkcache.s3.amazonaws.com/com.spotify.music_24382006.apk', diff --git a/cereal/car.capnp b/cereal/car.capnp index 5e406d6e..76b89c44 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -207,6 +207,7 @@ struct CarControl { brake @1: Float32; # range from -1.0 - 1.0 steer @2: Float32; + steerAngle @3: Float32; } struct CruiseControl { diff --git a/cereal/log.capnp b/cereal/log.capnp index f6df60cd..5996cf55 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -117,6 +117,8 @@ struct FrameData { frameLength @3 :Int32; integLines @4 :Int32; globalGain @5 :Int32; + lensPos @11 :Int32; + lensSag @12 :Float32; image @6 :Data; frameType @7 :FrameType; @@ -515,6 +517,7 @@ struct Plan { aCruise @17 :Float32; vTarget @3 :Float32; vTargetFuture @14 :Float32; + vMax @20 :Float32; aTargetMinDEPRECATED @4 :Float32; aTargetMaxDEPRECATED @5 :Float32; aTarget @18 :Float32; @@ -576,12 +579,14 @@ struct LiveLocationData { source @14 :SensorSource; # if we are fixing a location in the past fixMonoTime @15 :UInt64; - + gpsWeek @16 :Int32; timeOfWeek @17 :Float64; positionECEF @18 :List(Float64); poseQuatECEF @19 :List(Float32); + pitchCalibration @20 :Float32; + yawCalibration @21 :Float32; struct Accuracy { pNEDError @0 :List(Float32); @@ -599,6 +604,7 @@ struct LiveLocationData { kalman @1; orbslam @2; timing @3; + dummy @4; } } @@ -1283,6 +1289,11 @@ struct UbloxGnss { fitInterval @35 :Float64; toc @36 :Float64; + + ionoCoeffsValid @37 :Bool; + ionoAlpha @38 :List(Float64); + ionoBeta @39 :List(Float64); + } struct IonoData { @@ -1341,7 +1352,8 @@ struct GPSPlannerPoints { points @1 :List(ECEFPoint); valid @2 :Bool; trackName @3 :Text; - instructionProgress @4 :Float32; + speedLimit @4 :Float32; + accelTarget @5 :Float32; } struct GPSPlannerPlan { @@ -1349,22 +1361,29 @@ struct GPSPlannerPlan { poly @1 :List(Float32); trackName @2 :Text; speed @3 :Float32; + acceleration @4 :Float32; + points @5 :List(ECEFPoint); } -struct TrafficSigns { +struct TrafficEvent @0xacfa74a094e62626 { type @0 :Type; distance @1 :Float32; action @2 :Action; resuming @3 :Bool; enum Type { - light @0; + stopSign @0; + lightRed @1; + lightYellow @2; + lightGreen @3; + stopLight @4; } enum Action { none @0; yield @1; stop @2; + resumeReady @3; } } @@ -1378,6 +1397,97 @@ struct OrbslamCorrection { numInliers @5 :UInt32; } +struct OrbObservation { + observationMonoTime @0 :UInt64; + normalizedCoordinates @1 :List(Float32); + locationECEF @2 :List(Float64); + matchDistance @3: UInt32; +} + +struct UiNavigationEvent { + type @0: Type; + status @1: Status; + distanceTo @2: Float32; + endRoadPoint @3: ECEFPoint; + + enum Type { + none @0; + laneChangeLeft @1; + laneChangeRight @2; + mergeLeft @3; + mergeRight @4; + turnLeft @5; + turnRight @6; + } + + enum Status { + none @0; + passive @1; + approaching @2; + active @3; + } +} + +struct UiLayoutState { + activeApp @0 :App; + sidebarCollapsed @1 :Bool; + mapEnabled @2 :Bool; + + enum App { + home @0; + music @1; + nav @2; + } +} + +struct Joystick { + # convenient for debug and live tuning + axes @0: List(Float32); + buttons @1: List(Bool); +} + +struct OrbOdometry { + # timing first + startMonoTime @0 :UInt64; + endMonoTime @1 :UInt64; + + # fundamental matrix and error + f @2: List(Float64); + err @3: Float64; + + # number of inlier points + inliers @4: Int32; + + # for debug only + # indexed by endMonoTime features + # value is startMonoTime feature match + # -1 if no match + matches @5: List(Int16); +} + +struct OrbFeatures { + timestampEof @0 :UInt64; + # transposed arrays of normalized image coordinates + # len(xs) == len(ys) == len(descriptors) * 32 + xs @1 :List(Float32); + ys @2 :List(Float32); + descriptors @3 :Data; + octaves @4 :List(Int8); +} + +struct OrbKeyFrame { + # this is a globally unique id for the KeyFrame + id @0: UInt64; + + # this is the location of the KeyFrame + pos @1: ECEFPoint; + + # these are the features in the world + # len(dpos) == len(descriptors) * 32 + dpos @2 :List(ECEFPoint); + descriptors @3 :Data; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1425,9 +1535,20 @@ struct Event { gpsPlannerPoints @40 :GPSPlannerPoints; gpsPlannerPlan @41 :GPSPlannerPlan; applanixRaw @42 :Data; - trafficSigns @43 :List(TrafficSigns); + trafficEvents @43 :List(TrafficEvent); liveLocationTiming @44 :LiveLocationData; - orbslamCorrection @45 :OrbslamCorrection; + orbslamCorrectionDEPRECATED @45 :OrbslamCorrection; liveLocationCorrected @46 :LiveLocationData; + orbObservation @47 :List(OrbObservation); + gpsLocationExternal @48 :GpsLocationData; + location @49 :LiveLocationData; + uiNavigationEvent @50 :UiNavigationEvent; + liveLocationKalman @51 :LiveLocationData; + testJoystick @52 :Joystick; + orbOdometry @53 :OrbOdometry; + orbFeatures @54 :OrbFeatures; + applanixLocation @55 :LiveLocationData; + orbKeyFrame @56 :OrbKeyFrame; + uiLayoutState @57 :UiLayoutState; } } diff --git a/common/fingerprints.py b/common/fingerprints.py index 241dc50a..ee10abd0 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -14,43 +14,43 @@ class TOYOTA: COROLLA = "TOYOTA COROLLA 2017" LEXUS_RXH = "LEXUS RX HYBRID 2017" +_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes _FINGERPRINTS = { - HONDA.ACURA_ILX: { - 1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5, + HONDA.ACURA_ILX: [{ + 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5, # sent messages - 0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, - }, - HONDA.ACURA_RDX: { + 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, + }], + HONDA.ACURA_RDX: [{ 57L: 3, 145L: 8, 229L: 4, 308L: 5, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 398L: 3, 399L: 6, 404L: 4, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 892L: 8, 923L: 2, 929L: 4, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1034L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1365L: 5, 1424L: 5, 1729L: 1 - }, - HONDA.CIVIC: { - 1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5, + }], + HONDA.CIVIC: [{ + 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5, # sent messages - 0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, - }, - HONDA.CRV: { + 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, + }], + HONDA.CRV: [{ 57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, # sent messages 0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5, - }, + }], HONDA.ODYSSEY: [{ 57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 }, # Odyssey Elite { 57L: 3, 148L: 8, 228L: 5, 229L: 4, 304L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 440L: 8, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 - } - ], - HONDA.PILOT: { + }], + HONDA.PILOT: [{ 1600L: 5, 1027L: 5, 1668L: 5, 1029L: 8, 1601L: 8, 777L: 8, 891L: 8, 1036L: 8, 399L: 7, 1424L: 5, 145L: 8, 660L: 8, 985L: 3, 1616L: 5, 538L: 3, 795L: 8, 542L: 7, 773L: 7, 800L: 8, 545L: 5, 546L: 3, 419L: 8, 420L: 8, 422L: 8, 1064L: 7, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 819L: 7, 308L: 5, 821L: 5, 57L: 3, 965L: 8, 316L: 8, 829L: 5, 1088L: 8, 1089L: 8, 963L: 8, 837L: 5, 966L: 8, 929L: 8, 780L: 8, 923L: 2, 1613L: 5, 334L: 8, 463L: 8, 464L: 8, 1618L: 5, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 856L: 7, 804L: 8, 1612L: 5, 476L: 4, 1125L: 8, 344L: 8, 1296L: 8, 379L: 8, 228L: 5, 229L: 4, 871L: 8, 892L: 8, 490L: 8, 808L: 8, 882L: 2, 884L: 7, 967L: 8, 506L: 8, 507L: 1, 380L: 8, - }, - TOYOTA.RAV4: { + }], + TOYOTA.RAV4: [{ 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }, - TOYOTA.RAV4H: { - 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }, + }], + TOYOTA.RAV4H: [{ + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }], TOYOTA.PRIUS: [{ 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 }, @@ -61,14 +61,13 @@ _FINGERPRINTS = { # Taiwanese Prius Prime { 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 845L: 5, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1264L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - } - ], - TOYOTA.COROLLA: { + }], + TOYOTA.COROLLA: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 - }, - TOYOTA.LEXUS_RXH: { + }], + TOYOTA.LEXUS_RXH: [{ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 - }, + }], } # support additional internal only fingerprints @@ -97,10 +96,10 @@ def eliminate_incompatible_cars(msg, candidate_cars): compatible_cars = [] for car_name in candidate_cars: car_fingerprints = _FINGERPRINTS[car_name] - if not isinstance(car_fingerprints, list): - car_fingerprints = [car_fingerprints] for fingerprint in car_fingerprints: + fingerprint.update(_DEBUG_ADDRESS) # add alien debug address + if is_valid_for_fingerprint(msg, fingerprint): compatible_cars.append(car_name) break diff --git a/common/profiler.py b/common/profiler.py index 83e5672e..953f5737 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -6,7 +6,7 @@ class Profiler(object): self.cp = {} self.cp_ignored = [] self.iter = 0 - self.start_time = time.clock() + self.start_time = time.time() self.last_time = self.start_time self.tot = 0. @@ -15,14 +15,14 @@ class Profiler(object): self.cp = {} self.cp_ignored = [] self.iter = 0 - self.start_time = time.clock() + self.start_time = time.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() + tt = time.time() if name not in self.cp: self.cp[name] = 0. if ignore: @@ -37,11 +37,10 @@ class Profiler(object): return self.iter += 1 print "******* Profiling *******" - for n in self.cp: - ms = self.cp[n] + for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]): if n in self.cp_ignored: - print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED" + print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED" else: - print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100) + print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100) print "Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot) diff --git a/common/transformations/__init__.py b/common/transformations/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py new file mode 100644 index 00000000..14f92559 --- /dev/null +++ b/common/transformations/coordinates.py @@ -0,0 +1,104 @@ +import numpy as np +""" +Coordinate transformation module. All methods accept arrays as input +with each row as a position. +""" + + + +a = 6378137 +b = 6356752.3142 +esq = 6.69437999014 * 0.001 +e1sq = 6.73949674228 * 0.001 + + +def geodetic2ecef(geodetic): + geodetic = np.array(geodetic) + input_shape = geodetic.shape + geodetic = np.atleast_2d(geodetic) + lat = (np.pi/180)*geodetic[:,0] + lon = (np.pi/180)*geodetic[:,1] + alt = geodetic[:,2] + + xi = np.sqrt(1 - esq * np.sin(lat)**2) + x = (a / xi + alt) * np.cos(lat) * np.cos(lon) + y = (a / xi + alt) * np.cos(lat) * np.sin(lon) + z = (a / xi * (1 - esq) + alt) * np.sin(lat) + ecef = np.array([x, y, z]).T + return ecef.reshape(input_shape) + + +def ecef2geodetic(ecef): + """ + Convert ECEF coordinates to geodetic using ferrari's method + """ + def ferrari(x, y, z): + # ferrari's method + r = np.sqrt(x * x + y * y) + Esq = a * a - b * b + F = 54 * b * b * z * z + G = r * r + (1 - esq) * z * z - esq * Esq + C = (esq * esq * F * r * r) / (pow(G, 3)) + S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C)) + P = F / (3 * pow((S + 1 / S + 1), 2) * G * G) + Q = np.sqrt(1 + 2 * esq * esq * P) + r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \ + P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r) + U = np.sqrt(pow((r - esq * r_0), 2) + z * z) + V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z) + Z_0 = b * b * z / (a * V) + h = U * (1 - b * b / (a * V)) + lat = (180/np.pi)*np.arctan((z + e1sq * Z_0) / r) + lon = (180/np.pi)*np.arctan2(y, x) + return lat, lon, h + + geodetic = [] + ecef = np.array(ecef) + input_shape = ecef.shape + ecef = np.atleast_2d(ecef) + for p in ecef: + geodetic.append(ferrari(*p)) + geodetic = np.array(geodetic) + return geodetic.reshape(input_shape) + + + +class LocalCoord(object): + """ + Allows conversions to local frames. In this case NED. + That is: North East Down from the start position in + meters. + """ + def __init__(self, init_geodetic, init_ecef): + self.init_ecef = init_ecef + lat, lon, _ = (np.pi/180)*init_geodetic + self.ned2ecef_matrix = np.array([[-np.sin(lat)*np.cos(lon), -np.sin(lon), -np.cos(lat)*np.cos(lon)], + [-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)], + [np.cos(lat), 0, -np.sin(lat)]]) + self.ecef2ned_matrix = self.ned2ecef_matrix.T + + @classmethod + def from_geodetic(self, init_geodetic): + init_ecef = geodetic2ecef(init_geodetic) + return LocalCoord(init_geodetic, init_ecef) + + @classmethod + def from_ecef(self, init_ecef): + init_geodetic = ecef2geodetic(init_ecef) + return LocalCoord(init_geodetic, init_ecef) + + + def ecef2ned(self, ecef): + return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T + + def ned2ecef(self, ned): + # Transpose so that init_ecef will broadcast correctly for 1d or 2d ned. + return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef) + + def geodetic2ned(self, geodetic): + ecef = geodetic2ecef(geodetic) + return self.ecef2ned(ecef) + + def ned2geodetic(self, ned): + ecef = self.ned2ecef(ned) + return ecef2geodetic(ecef) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh new file mode 100755 index 00000000..3959c58d --- /dev/null +++ b/launch_chffrplus.sh @@ -0,0 +1,44 @@ +#!/usr/bin/bash + +if [ -z "$PASSIVE" ]; then + export PASSIVE="1" +fi + +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 + + # 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 + + # check if NEOS update is required + while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do + # 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 + + # update NEOS + curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater + sleep 10 + done + + export PYTHONPATH="$PWD" + + # start manager + cd selfdrive + ./manager.py + + # if broken, keep on screen error + while true; do sleep 1; done +} + +launch diff --git a/launch_openpilot.sh b/launch_openpilot.sh index 2e7b163c..1525e171 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,39 +1,5 @@ #!/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 +export PASSIVE="0" +exec ./launch_chffrplus.sh - # 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 - - # 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 - - export PYTHONPATH="$PWD" - - # start manager - cd selfdrive - ./manager.py - - # if broken, keep on screen error - while true; do sleep 1; done -} - -launch diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index 2779fab8..a005faac 100644 --- a/requirements_openpilot.txt +++ b/requirements_openpilot.txt @@ -14,4 +14,5 @@ enum34==1.1.1 sympy==1.1.1 filterpy==1.0.0 smbus2==0.2.0 +pyflakes==1.5.0 -e git+https://github.com/commaai/le_python.git#egg=Logentries diff --git a/selfdrive/assets/OpenSans-Bold.ttf b/selfdrive/assets/OpenSans-Bold.ttf new file mode 100644 index 00000000..7b529456 Binary files /dev/null and b/selfdrive/assets/OpenSans-Bold.ttf differ diff --git a/selfdrive/assets/img_chffr_wheel.png b/selfdrive/assets/img_chffr_wheel.png new file mode 100644 index 00000000..3f09a35a Binary files /dev/null and b/selfdrive/assets/img_chffr_wheel.png differ diff --git a/selfdrive/assets/img_trafficLight_green.png b/selfdrive/assets/img_trafficLight_green.png new file mode 100644 index 00000000..b2e07dab Binary files /dev/null and b/selfdrive/assets/img_trafficLight_green.png differ diff --git a/selfdrive/assets/img_trafficLight_none.png b/selfdrive/assets/img_trafficLight_none.png new file mode 100644 index 00000000..01deb5a0 Binary files /dev/null and b/selfdrive/assets/img_trafficLight_none.png differ diff --git a/selfdrive/assets/img_trafficLight_red.png b/selfdrive/assets/img_trafficLight_red.png new file mode 100644 index 00000000..4c440250 Binary files /dev/null and b/selfdrive/assets/img_trafficLight_red.png differ diff --git a/selfdrive/assets/img_trafficLight_yellow.png b/selfdrive/assets/img_trafficLight_yellow.png new file mode 100644 index 00000000..8dd5bd42 Binary files /dev/null and b/selfdrive/assets/img_trafficLight_yellow.png differ diff --git a/selfdrive/assets/img_trafficSign_stop.png b/selfdrive/assets/img_trafficSign_stop.png new file mode 100644 index 00000000..9261c474 Binary files /dev/null and b/selfdrive/assets/img_trafficSign_stop.png differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 30ff6b40..d52c4f34 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -525,10 +525,25 @@ void pigeon_init() { pigeon_send("\xB5\x62\x06\x1E\x00\x00\x24\x72"); pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"); pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"); + pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"); LOGW("grey panda is ready to fly"); } +static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) { + // create message + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto ublox_raw = event.initUbloxRaw(alen); + memcpy(ublox_raw.begin(), dat, alen); + + // send to ubloxRaw + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(publisher, bytes.begin(), bytes.size(), 0); +} + void *pigeon_thread(void *crap) { // ubloxRaw = 8042 @@ -543,22 +558,6 @@ void *pigeon_thread(void *crap) { if (pigeon_needs_init) { pigeon_needs_init = false; pigeon_init(); - } else { - // send periodic messages - if (cnt%3000 == 0) { - for (unsigned char sv = 1; sv < 33; ++sv){ - const unsigned char buffer[5] = {0x0B, 0x31, 0x01, 0x00, sv}; - unsigned char CK_A = 0; - unsigned char CK_B = 0; - for(int i=0;i<5;i++) { - CK_A = CK_A + buffer[i]; - CK_B = CK_B + CK_A; - } - const unsigned char msg[9] = {0xB5, 0x62, 0x0B, 0x31, 0x01, 0x00, sv, CK_A, CK_B}; - _pigeon_send((const char *)msg, 9); - } - pigeon_send("\xB5\x62\x0b\x02\x00\x00\x0d\x32"); - } } int alen = 0; while (alen < 0xfc0) { @@ -572,17 +571,7 @@ void *pigeon_thread(void *crap) { alen += len; } if (alen > 0) { - // create message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto ublox_raw = event.initUbloxRaw(alen); - memcpy(ublox_raw.begin(), dat, alen); - - // send to ubloxRaw - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(publisher, bytes.begin(), bytes.size(), 0); + pigeon_publish_raw(publisher, dat, alen); } // 10ms diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 0b428d04..243e5395 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -101,7 +101,7 @@ def can_init(): if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc: handle = device.open() handle.claimInterface(0) - handle.controlWrite(0x40, 0xdc, SAFETY_HONDA, 0, b'') + handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') if handle is None: print "CAN NOT FOUND" diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py index 7495c88a..3c5d440f 100644 --- a/selfdrive/can/libdbc_py.py +++ b/selfdrive/can/libdbc_py.py @@ -1,12 +1,11 @@ import os -import sys import subprocess from cffi import FFI can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") -subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir) +subprocess.check_call(["make"], cwd=can_dir) ffi = FFI() ffi.cdef(""" @@ -67,7 +66,8 @@ typedef struct { void* can_init(int bus, const char* dbc_name, size_t num_message_options, const MessageParseOptions* message_options, - size_t num_signal_options, const SignalParseOptions* signal_options, bool sendcan); + size_t num_signal_options, const SignalParseOptions* signal_options, bool sendcan, + const char* tcp_addr); void can_update(void* can, uint64_t sec, bool wait); diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 5ea6e344..705fe709 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -3,6 +3,7 @@ import numbers from selfdrive.can.libdbc_py import libdbc, ffi + class CANPacker(object): def __init__(self, dbc_name): self.packer = libdbc.canpack_init(dbc_name) @@ -24,6 +25,9 @@ class CANPacker(object): # values: [(signal_name, signal_value)] values_thing = [] + if isinstance(values, dict): + values = values.items() + for name, value in values: if name not in self.sig_names: self.sig_names[name] = ffi.new("char[]", name) @@ -42,7 +46,9 @@ class CANPacker(object): size = self.address_to_size[addr] else: addr, size = self.name_to_address_and_size[addr] - r = struct.pack(">Q", self.pack(addr, values, counter)) + + val = self.pack(addr, values, counter) + r = struct.pack(">Q", val) return addr, r[:size] def make_can_msg(self, addr, bus, values, counter=-1): diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index a32844fe..52d7ceba 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -142,18 +142,23 @@ class CANParser { CANParser(int abus, const std::string& dbc_name, const std::vector &options, const std::vector &sigoptions, - bool sendcan) + bool sendcan, const std::string& tcp_addr) : bus(abus) { // connect to can on 8006 context = zmq_ctx_new(); subscriber = zmq_socket(context, ZMQ_SUB); zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); + std::string tcp_addr_str; + if (sendcan) { - zmq_connect(subscriber, "tcp://127.0.0.1:8017"); + tcp_addr_str = "tcp://" + tcp_addr + ":8017"; } else { - zmq_connect(subscriber, "tcp://127.0.0.1:8006"); + tcp_addr_str = "tcp://" + tcp_addr + ":8006"; } + const char *tcp_addr_char = tcp_addr_str.c_str(); + + zmq_connect(subscriber, tcp_addr_char); dbc = dbc_lookup(dbc_name); assert(dbc); @@ -331,12 +336,12 @@ extern "C" { void* can_init(int bus, const char* dbc_name, size_t num_message_options, const MessageParseOptions* message_options, size_t num_signal_options, const SignalParseOptions* signal_options, - bool sendcan) { + bool sendcan, const char* tcp_addr) { CANParser* ret = new CANParser(bus, std::string(dbc_name), (message_options ? std::vector(message_options, message_options+num_message_options) : std::vector{}), (signal_options ? std::vector(signal_options, signal_options+num_signal_options) - : std::vector{}), sendcan); + : std::vector{}), sendcan, std::string(tcp_addr)); return (void*)ret; } diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 13603b78..4806e1a1 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -5,11 +5,12 @@ import numbers from selfdrive.can.libdbc_py import libdbc, ffi class CANParser(object): - def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False): + def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False, tcp_addr="127.0.0.1"): self.can_valid = True self.vl = defaultdict(dict) self.ts = defaultdict(dict) + self.dbc_name = dbc_name self.dbc = libdbc.dbc_lookup(dbc_name) self.msg_name_to_addres = {} self.address_to_msg_name = {} @@ -39,10 +40,6 @@ class CANParser(object): sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals) - # Set default values by name - for sig_name, sig_address, sig_default in signals: - self.vl[self.address_to_msg_name[sig_address]][sig_name] = sig_default - signal_options_c = ffi.new("SignalParseOptions[]", [ { 'address': sig_address, @@ -60,7 +57,7 @@ class CANParser(object): } for msg_address, freq in message_options.iteritems()]) self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, - len(signal_options_c), signal_options_c, sendcan) + len(signal_options_c), signal_options_c, sendcan, tcp_addr) self.p_can_valid = ffi.new("bool*") diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index dc23457e..e1a492bb 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -6,6 +6,7 @@ from common.numpy_fast import clip from . import hondacan from .values import AH from common.fingerprints import HONDA as CAR +from selfdrive.can.packer import CANPacker def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): @@ -52,16 +53,17 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", - ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5", - "lanes", "beep", "X8", "chime", "acc_alert"]) + ["pcm_accel", "v_cruise", "mini_car", "car", "X4", + "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"]) class CarController(object): - def __init__(self, enable_camera=True): + def __init__(self, dbc_name, enable_camera=True): self.braking = False self.brake_steady = 0. self.brake_last = 0. self.enable_camera = enable_camera + self.packer = CANPacker(dbc_name) def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -85,36 +87,33 @@ class CarController(object): self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100) # vehicle hud display, wait for one update from 10Hz 0x304 msg - #TODO: use enum!! if hud_show_lanes: - hud_lanes = 0x04 + hud_lanes = 1 else: - hud_lanes = 0x00 + hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: - hud_car = 0xe0 + hud_car = 2 else: - hud_car = 0xd0 + hud_car = 1 else: - hud_car = 0xc0 + hud_car = 0 #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 0x01, hud_car, - 0xc1, 0x41, hud_lanes + steer_required, - int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert) + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, + 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud - hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0) + hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** - GAS_MAX = 1004 BRAKE_MAX = 1024/4 if CS.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.PILOT): is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] @@ -123,10 +122,9 @@ class CarController(object): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0xF00 - GAS_OFFSET = 328 # steer torque is converted back to CAN reference (positive when steering right) - apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1)) + apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) @@ -139,24 +137,23 @@ class CarController(object): # Send steering command. idx = frame % 4 - can_sends.extend(hondacan.create_steering_control(apply_steer, CS.CP.carFingerprint, idx)) + can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CS.CP.carFingerprint, idx)) # Send gas and brake commands. if (frame % 2) == 0: idx = (frame / 2) % 4 can_sends.append( - hondacan.create_brake_command(apply_brake, pcm_override, - pcm_cancel_cmd, hud.chime, idx)) + hondacan.create_brake_command(self.packer, apply_brake, pcm_override, + pcm_cancel_cmd, hud.chime, hud.fcw, idx)) if not CS.brake_only: # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling - gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0) - can_sends.append(hondacan.create_gas_command(gas_amount, idx)) + can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 - can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.CP.carFingerprint, idx)) + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) if CS.CP.carFingerprint == CAR.ACURA_ILX: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index e98596f2..7bd126c1 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -8,22 +8,25 @@ import numpy as np def parse_gear_shifter(can_gear_shifter, car_fingerprint): - # TODO: Use values from DBC to parse this field - if can_gear_shifter == 0x1: - return "park" - elif can_gear_shifter == 0x2: - return "reverse" + # TODO: Use VAL from DBC to parse this field if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY): - if can_gear_shifter == 0x3: + if can_gear_shifter == 0x1: + return "park" + elif can_gear_shifter == 0x2: + return "reverse" + elif can_gear_shifter == 0x3: return "neutral" elif can_gear_shifter == 0x4: return "drive" elif can_gear_shifter == 0xa: return "sport" - elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX): - if can_gear_shifter == 0x4: + if can_gear_shifter == 0x1: + return "park" + elif can_gear_shifter == 0x2: + return "reverse" + elif can_gear_shifter == 0x4: return "neutral" elif can_gear_shifter == 0x8: return "drive" @@ -31,8 +34,8 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint): return "sport" elif can_gear_shifter == 0x20: return "low" - elif car_fingerprint in (CAR.PILOT): + # TODO: neutral? if can_gear_shifter == 0x8: return "reverse" elif can_gear_shifter == 0x4: @@ -215,7 +218,7 @@ class CarState(object): # blend in transmission speed at low speed, since it has more low speed accuracy self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v) - speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] + self.v_weight * self.v_wheel + speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_x = np.matrix([[speed], [0.0]]) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 1758232f..a19134e5 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -4,7 +4,6 @@ import common.numpy_fast as np from selfdrive.config import Conversions as CV from common.fingerprints import HONDA as CAR - # *** Honda specific *** def can_cksum(mm): s = 0 @@ -29,56 +28,89 @@ def make_can_msg(addr, dat, idx, alt): return [addr, 0, dat, alt] -def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx): +def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chime, fcw, idx): """Creates a CAN message for the Honda DBC BRAKE_COMMAND.""" pump_on = apply_brake > 0 brakelights = apply_brake > 0 brake_rq = apply_brake > 0 - pcm_fault_cmd = False - amount = struct.pack("!H", (apply_brake << 6) + pump_on) - msg = amount + struct.pack("BBB", (pcm_override << 4) | - (pcm_fault_cmd << 2) | - (pcm_cancel_cmd << 1) | brake_rq, 0x80, - brakelights << 7) + chr(chime) + "\x00" - return make_can_msg(0x1fa, msg, idx, 0) + + values = { + "COMPUTER_BRAKE": apply_brake, + "COMPUTER_BRAKE_REQUEST": pump_on, + "CRUISE_OVERRIDE": pcm_override, + "CRUISE_FAULT_CMD": pcm_fault_cmd, + "CRUISE_CANCEL_CMD": pcm_cancel_cmd, + "COMPUTER_BRAKE_REQUEST_2": brake_rq, + "SET_ME_0X80": 0x80, + "BRAKE_LIGHTS": brakelights, + "CHIME": chime, + "FCW": fcw << 1, # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work + } + return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx) -def create_gas_command(gas_amount, idx): +def create_gas_command(packer, gas_amount, idx): """Creates a CAN message for the Honda DBC GAS_COMMAND.""" - msg = struct.pack("!H", gas_amount) - return make_can_msg(0x200, msg, idx, 0) + enable = gas_amount > 0.001 -def create_steering_control(apply_steer, car_fingerprint, idx): + values = {"ENABLE": enable} + + if enable: + values["GAS_COMMAND"] = gas_amount * 255. + values["GAS_COMMAND2"] = gas_amount * 255. + + return packer.make_can_msg("GAS_COMMAND", 0, values, idx) + + +def create_steering_control(packer, apply_steer, car_fingerprint, idx): """Creates a CAN message for the Honda DBC STEERING_CONTROL.""" - commands = [] - if car_fingerprint in (CAR.CRV, CAR.ACURA_RDX): - msg_0x194 = struct.pack("!h", apply_steer << 4) + ("\x80" if apply_steer != 0 else "\x00") - commands.append(make_can_msg(0x194, msg_0x194, idx, 0)) - else: - msg_0xe4 = struct.pack("!h", apply_steer) + ("\x80\x00" if apply_steer != 0 else "\x00\x00") - commands.append(make_can_msg(0xe4, msg_0xe4, idx, 0)) - return commands + values = { + "STEER_TORQUE": apply_steer, + "STEER_TORQUE_REQUEST": apply_steer != 0, + } + return packer.make_can_msg("STEERING_CONTROL", 0, values, idx) -def create_ui_commands(pcm_speed, hud, car_fingerprint, idx): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): """Creates an iterable of CAN messages for the UIs.""" commands = [] - pcm_speed_real = np.clip(int(round(pcm_speed / 0.002759506)), 0, - 64000) # conversion factor from dbc file - msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel, - hud.v_cruise, hud.X2, hud.car, hud.X4) - commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0)) - msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8) - commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0)) + # TODO: Why is X4 always 0xc1? Not implemented yet in canpacker + acc_hud_values = { + 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, + 'PCM_GAS': hud.pcm_accel, + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': hud.mini_car, + 'HUD_LEAD': hud.car, + 'SET_ME_X03': 0x03, + 'SET_ME_X03_2': 0x03, + 'SET_ME_X01': 0x01, + } + commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) + + lkas_hud_values = { + 'SET_ME_X41': 0x41, + 'SET_ME_X48': 0x48, + 'STEERING_REQUIRED': hud.steer_required, + 'SOLID_LANES': hud.lanes, + 'BEEP': hud.beep, + } + commands.append(packer.make_can_msg('LKAS_HUD', 0, lkas_hud_values, idx)) + if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY): - msg_0x35e = chr(0) * 7 - commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0)) - msg_0x39f = (chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)) - commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0)) + commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx)) + + radar_hud_values = { + 'ACC_ALERTS': hud.acc_alert, + 'LEAD_SPEED': 0x1fe, # What are these magic values + 'LEAD_STATE': 0x7, + 'LEAD_DISTANCE': 0x1e, + } + commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx)) return commands + def create_radar_commands(v_ego, car_fingerprint, idx): """Creates an iterable of CAN messages for the radar system.""" commands = [] @@ -106,7 +138,7 @@ def create_radar_commands(v_ego, car_fingerprint, idx): commands.append(make_can_msg(0x300, msg_0x300, idx, 1)) elif car_fingerprint == CAR.PILOT: msg_0x301 = "\x00\x00\x56\x02\x58\x00\x00" - commands.append(make_can_msg(0x300, msg_0x300, idx, 1)) + commands.append(make_can_msg(0x300, msg_0x300, idx, 1)) commands.append(make_can_msg(0x301, msg_0x301, idx, 1)) return commands diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 18323b83..800b82c1 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -97,7 +97,7 @@ class CarInterface(object): # sending if read only is False if sendcan is not None: self.sendcan = sendcan - self.CC = CarController(CP.enableCamera) + self.CC = CarController(self.cp.dbc_name, CP.enableCamera) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() @@ -106,6 +106,10 @@ class CarInterface(object): @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): + # limit the pcm accel cmd if: + # - v_ego exceeds v_target, or + # - a_ego exceeds a_target and v_ego is close to v_target + eA = a_ego - a_target valuesA = [1.0, 0.1] bpA = [0.3, 1.1] @@ -114,9 +118,17 @@ class CarInterface(object): valuesV = [1.0, 0.1] bpV = [0.0, 0.5] + valuesRangeV = [1., 0.] + bpRangeV = [-1., 0.] + + # only limit if v_ego is close to v_target + speedLimiter = interp(eV, bpV, valuesV) + accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) + # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart - return float(max(0.714, a_target / A_ACC_MAX)) * min(interp(eA, bpA, valuesA), interp(eV, bpV, valuesV)) + + return float(max(0.714, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint): @@ -174,7 +186,7 @@ class CarInterface(object): ret.steerRatio = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310'] - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.1], [0.03]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -203,7 +215,7 @@ class CarInterface(object): ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4354./2.205 + std_cargo @@ -431,9 +443,12 @@ class CarInterface(object): # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low - # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: - events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) + # non loud alert if cruise disbales below 25mph as expected (+ a little margin) + if ret.vEgo < self.CP.minEnableSpeed + 2.: + events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) + else: + events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.carFingerprint != CAR.CIVIC and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index ee5c0888..35db0c21 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -26,7 +26,7 @@ class AH: #[alert_idx, value] # See dbc files for info on values" NONE = [0, 0] - FCW = [1, 0x8] + FCW = [1, 1] STEER = [2, 1] BRAKE_PRESSED = [3, 10] GEAR_NOT_D = [4, 6] diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 3fd19f94..691cc3dd 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,26 +1,36 @@ -from common.numpy_fast import clip +from common.numpy_fast import clip, interp, int_rnd from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command from selfdrive.car.toyota.values import ECU, STATIC_MSGS +from selfdrive.can.packer import CANPacker +# Accel limits ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value -ACCEL_MAX = 1500 # 1.5 m/s2 -ACCEL_MIN = -3000 # 3 m/s2 +ACCEL_MAX = 1.5 # 1.5 m/s2 +ACCEL_MIN = -3.0 # 3 m/s2 ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) +# Steer torque limits STEER_MAX = 1500 -STEER_DELTA_UP = 10 # 1.5s time to peak torque -STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) -STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor +STEER_DELTA_UP = 10 # 1.5s time to peak torque +STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) +STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + +# Steer angle limits +ANGLE_MAX_BP = [0., 5.] +ANGLE_MAX_V = [510., 300.] +ANGLE_DELTA_BP = [0., 5.] +ANGLE_DELTA_V = [3., 1.] TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345, 0x363, 0x364, 0x365, 0x370, 0x371, 0x372, 0x373, 0x374, 0x375, 0x380, 0x381, 0x382, 0x383] + def accel_hysteresis(accel, accel_steady, enabled): # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command @@ -56,35 +66,65 @@ def process_hud_alert(hud_alert, audible_alert): return steer, fcw, sound1, sound2 +def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_counter): + + if enabled and not steer_angle_enabled: + #ipas_reset_counter = max(0, ipas_reset_counter - 1) + #if ipas_reset_counter == 0: + # steer_angle_enabled = True + #else: + # steer_angle_enabled = False + #return steer_angle_enabled, ipas_reset_counter + return True, 0 + + elif enabled and steer_angle_enabled: + if steer_angle_enabled and ipas_state != 3: + ipas_reset_counter += 1 + else: + ipas_reset_counter = 0 + if ipas_reset_counter > 10: # try every 0.1s + steer_angle_enabled = False + return steer_angle_enabled, ipas_reset_counter + + else: + return False, 0 + class CarController(object): - def __init__(self, car_fingerprint, enable_camera, enable_dsu, enable_apg): + def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg): self.braking = False # redundant safety check with the board self.controls_allowed = True - self.last_steer = 0. + self.last_steer = 0 + self.last_angle = 0 self.accel_steady = 0. self.car_fingerprint = car_fingerprint self.alert_active = False self.last_standstill = False self.standstill_req = False + self.angle_control = False + + self.steer_angle_enabled = False + self.ipas_reset_counter = 0 self.fake_ecus = set() if enable_camera: self.fake_ecus.add(ECU.CAM) if enable_dsu: self.fake_ecus.add(ECU.DSU) if enable_apg: self.fake_ecus.add(ECU.APGS) + self.packer = CANPacker(dbc_name) + def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert): # *** compute control surfaces *** - # steer torque is converted back to CAN reference (positive when steering right) + # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) - apply_accel = int(round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX))) + apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) - # steer torque is converted back to CAN reference (positive when steering right) + # steer torque apply_steer = int(round(actuators.steer * STEER_MAX)) max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) @@ -98,15 +138,31 @@ class CarController(object): else: apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP)) + # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota + # cuts steer torque immediately anyway TODO: monitor if this is a real issue + # only cut torque when steer state is a known fault + if not enabled or CS.steer_state in [18, 50]: + apply_steer = 0 + + self.steer_angle_enabled, self.ipas_reset_counter = \ + ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_state, self.ipas_reset_counter) + #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_state + + # steer angle + if self.steer_angle_enabled: + apply_angle = actuators.steerAngle + angle_lim = int_rnd(interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)) + apply_angle = clip(apply_angle, -angle_lim, angle_lim) + + angle_rate_lim = int_rnd(interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)) + apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + else: + apply_angle = CS.angle_steers + if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 - # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota - # cuts steer torque immediately anyway TODO: monitor if this is a real issue - if not enabled or CS.steer_error: - apply_steer = 0 - # on entering standstill, send standstill request if CS.standstill and not self.last_standstill: self.standstill_req = True @@ -115,6 +171,7 @@ class CarController(object): self.standstill_req = False self.last_steer = apply_steer + self.last_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill @@ -127,15 +184,21 @@ class CarController(object): # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if ECU.CAM in self.fake_ecus: - can_sends.append(create_steer_command(apply_steer, frame)) + if self.angle_control: + can_sends.append(create_steer_command(self.packer, 0., frame)) + else: + can_sends.append(create_steer_command(self.packer, apply_steer, frame)) if ECU.APGS in self.fake_ecus: - can_sends.append(create_ipas_steer_command(apply_steer)) + if self.angle_control: + can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled)) + else: + can_sends.append(create_ipas_steer_command(self.packer, 0, 0)) # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): if ECU.DSU in self.fake_ecus: - can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd, self.standstill_req)) + can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) else: can_sends.append(create_accel_command(0, pcm_cancel_cmd, False)) @@ -157,8 +220,8 @@ class CarController(object): send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: - can_sends.append(create_ui_command(steer, sound1, sound2)) - can_sends.append(create_fcw_command(fcw)) + can_sends.append(create_ui_command(self.packer, steer, sound1, sound2)) + can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 24fc8523..06df6e67 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -75,6 +75,7 @@ def get_can_parser(CP): ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers ("LKA_STATE", "EPS_STATUS", 0), + ("IPAS_STATE", "EPS_STATUS", 1), ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), ] @@ -125,9 +126,6 @@ class CarState(object): cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] - self.steer_error = False - self.brake_error = 0 - can_gear = cp.vl["GEAR_PACKET"]['GEAR'] self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] @@ -160,7 +158,11 @@ class CarState(object): # we could use the override bit from dbc, but it's triggered at too high torque values self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100 - self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] == 50 + # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state + self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] + self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [2, 10] + self.ipas_state = cp.vl['EPS_STATUS']['IPAS_STATE'] + self.brake_error = 0 self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index acd22e9a..55874788 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,7 +33,7 @@ class CarInterface(object): # sending if read only is False if sendcan is not None: self.sendcan = sendcan - self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) + self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) @staticmethod def compute_gb(accel, speed): @@ -76,11 +76,15 @@ class CarInterface(object): if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 - ret.steerRatio = 14.5 # TODO: find exact value for Prius + ret.steerRatio = 15.0 ret.mass = 3045./2.205 + std_cargo - ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] + ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 - ret.steerRateCost = 2. + ret.steerRateCost = 1.5 + + f = 1.43353663 + tireStiffnessFront_civic *= f + tireStiffnessRear_civic *= f elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 @@ -146,11 +150,10 @@ class CarInterface(object): ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) - ret.enableApgs = False # not check_ecu_msgs(fingerprint, candidate, ECU.APGS) + ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS) print "ECU Camera Simulated: ", ret.enableCamera print "ECU DSU Simulated: ", ret.enableDsu print "ECU APGS Simulated: ", ret.enableApgs - ret.enableGas = True ret.steerLimitAlert = False ret.stoppingControl = False diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index c4c64e45..4cb2c0c8 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -28,62 +28,73 @@ def create_video_target(frame, addr): return make_can_msg(addr, msg, 1, True) -def create_ipas_steer_command(steer): - +def create_ipas_steer_command(packer, steer, enabled): """Creates a CAN message for the Toyota Steer Command.""" if steer < 0: - move = 0x60 - steer = 0xfff + steer + 1 + direction = 3 elif steer > 0: - move = 0x20 + direction = 1 else: - move = 0x40 + direction = 2 - mode = 0x30 if steer else 0x10 + mode = 3 if enabled else 1 - steer_h = (steer & 0xF00) >> 8 - steer_l = steer & 0xff + values = { + "STATE": mode, + "DIRECTION_CMD": direction, + "ANGLE": steer, + "SET_ME_X10": 0x10, + "SET_ME_X40": 0x40 + } + return packer.make_can_msg("STEERING_IPAS", 0, values) - msg = struct.pack("!BBBBBBB", mode | steer_h, steer_l, 0x10, 0x00, move, 0x40, 0x00) - return make_can_msg(0x266, msg, 0, True) - -def create_steer_command(steer, raw_cnt): +def create_steer_command(packer, steer, raw_cnt): """Creates a CAN message for the Toyota Steer Command.""" - # from 0x80 to 0xff - counter = ((raw_cnt & 0x3f) << 1) | 0x80 - if steer != 0: - counter |= 1 - # hud - # 00 => Regular - # 40 => Actively Steering (with beep) - # 80 => Actively Steering (without beep) - hud = 0x00 - - msg = struct.pack("!BhB", counter, steer, hud) - - return make_can_msg(0x2e4, msg, 0, True) + values = { + "STEER_REQUEST": abs(steer) > 0.001, + "STEER_TORQUE_CMD": steer, + "COUNTER": raw_cnt, + "SET_ME_1": 1, + } + return packer.make_can_msg("STEERING_LKA", 0, values) -def create_accel_command(accel, pcm_cancel, standstill_req): +def create_accel_command(packer, accel, pcm_cancel, standstill_req): # TODO: find the exact canceling bit - state = 0x40 if standstill_req else 0xC0 - state += pcm_cancel # this allows automatic restart from hold without driver cmd - - msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00) - - return make_can_msg(0x343, msg, 0, True) - -def create_fcw_command(fcw): - - msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00) - - return make_can_msg(0x411, msg, 0, False) + values = { + "ACCEL_CMD": accel, + "SET_ME_X63": 0x63, + "SET_ME_1": 1, + "RELEASE_STANDSTILL": not standstill_req, + "CANCEL_REQ": pcm_cancel, + } + return packer.make_can_msg("ACC_CONTROL", 0, values) -def create_ui_command(steer, sound1, sound2): +def create_fcw_command(packer, fcw): + values = { + "FCW": fcw, + "SET_ME_X20": 0x20, + "SET_ME_X10": 0x10, + "SET_ME_X80": 0x80, + } + return packer.make_can_msg("ACC_HUD", 0, values) - msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00, - sound1, 0x2C, 0x38, 0x02) - return make_can_msg(0x412, msg, 0, False) + +def create_ui_command(packer, steer, sound1, sound2): + values = { + "RIGHT_LINE": 1, + "LEFT_LINE": 1, + "SET_ME_X0C": 0x0c, + "SET_ME_X2C": 0x2c, + "SET_ME_X38": 0x38, + "SET_ME_X02": 0x02, + "SET_ME_X01": 1, + "SET_ME_X01_2": 1, + "REPEATED_BEEPS": sound1, + "TWO_BEEPS": sound2, + "LDA_ALERT": steer, + } + return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 348ff459..6721f927 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.4.2-openpilot" +#define COMMA_VERSION "0.4.3-release" diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index 24b88234..2f116a4d 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -48,7 +48,9 @@ typedef struct VisionStreamBufs { } VisionStreamBufs; typedef struct VIPCBufExtra { - uint32_t frame_id; // only for yuv + // only for yuv + uint32_t frame_id; + uint64_t timestamp_eof; } VIPCBufExtra; typedef union VisionPacketData { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 483868aa..89703f9b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -273,8 +273,8 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, CP, PL.lead_1) # *** steering PID loop *** - actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle, - CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, + CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) # send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -454,6 +454,11 @@ def controlsd_thread(gctx, rate=100): CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) + # if stock camera is connected, then force passive behavior + if not CP.enableCamera: + passive = True + sendcan = None + if CI is None: raise Exception("unsupported car") diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 82174135..5c82c240 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -15,14 +15,14 @@ AlertSize = log.Live100Data.AlertSize AlertStatus = log.Live100Data.AlertStatus class Alert(object): - def __init__(self, + def __init__(self, alert_text_1, alert_text_2, alert_status, alert_size, alert_priority, visual_alert, - audible_alert, + audible_alert, duration_sound, duration_hud_alert, duration_text): @@ -34,7 +34,7 @@ class Alert(object): self.alert_priority = alert_priority self.visual_alert = visual_alert if visual_alert is not None else "none" self.audible_alert = audible_alert if audible_alert is not None else "none" - + self.duration_sound = duration_sound self.duration_hud_alert = duration_hud_alert self.duration_text = duration_text @@ -71,356 +71,356 @@ class AlertManager(object): Priority.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "Brake!", - "Risk of Collision", + "Brake!", + "Risk of collision detected", AlertStatus.critical, AlertSize.full, Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( - "Take Control", - "Turn Exceeds Limit", - AlertStatus.userPrompt, AlertSize.full, + "TAKE CONTROL", + "Turn exceeds steering limit", + AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), "steerTempUnavailable": Alert( - "Take Control", - "Steer Temporarily Unavailable", - AlertStatus.userPrompt, AlertSize.full, + "TAKE CONTROL", + "Steering temporarily unavailable", + AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), "preDriverDistracted": Alert( - "Take Control", - "User Distracted", - AlertStatus.userPrompt, AlertSize.full, + "TAKE CONTROL", + "User appears distracted", + AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1), "driverDistracted": Alert( - "Take Control to Regain Speed", - "User Distracted", + "TAKE CONTROL TO REGAIN SPEED", + "User appears distracted", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), "startup": Alert( - "Always Keep Hands on Wheel", - "Be Ready to Take Over Any Time", - AlertStatus.normal, AlertSize.full, + "Always keep hands on wheel", + "Be ready to take over at any time", + AlertStatus.normal, AlertSize.mid, Priority.LOWEST, None, None, 0., 0., 15.), "ethicalDilemma": Alert( - "Take Control Immediately", - "Ethical Dilemma Detected", + "TAKE CONTROL IMMEDIATELY", + "Ethical Dilemma Detected", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), "steerTempUnavailableNoEntry": Alert( - "Comma Unavailable", - "Steer Temporary Unavailable", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Steering temporarily unavailable", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "manualRestart": Alert( - "Take Control", - "Resume Driving Manually", + "TAKE CONTROL", + "Resume driving manually", AlertStatus.userPrompt, AlertSize.full, Priority.LOW, None, None, 0., 0., .2), # Non-entry only alerts "wrongCarModeNoEntry": Alert( - "Comma Unavailable", - "Main Switch Off", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Main Switch Off", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "dataNeededNoEntry": Alert( - "Comma Unavailable", - "Data needed for calibration. Upload drive, try again", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Data needed for calibration. Upload drive, try again", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "outOfSpaceNoEntry": Alert( - "Comma Unavailable", - "Out of Space", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Out of storage space", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "pedalPressedNoEntry": Alert( - "Comma Unavailable", - "Pedal Pressed", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Pedal pressed during attempt", + AlertStatus.normal, AlertSize.mid, Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), "speedTooLowNoEntry": Alert( - "Comma Unavailable", - "Speed Too Low", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Speed too low", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeHoldNoEntry": Alert( - "Comma Unavailable", - "Brake Hold Active", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Brake hold active", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "parkBrakeNoEntry": Alert( - "Comma Unavailable", - "Park Brake Engaged", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Park brake engaged", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "lowSpeedLockoutNoEntry": Alert( "Comma Unavailable", "Cruise Fault: Restart the Car", - AlertStatus.normal, AlertSize.full, + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), # Cancellation alerts causing soft disabling "overheat": Alert( - "Take Control Immediately", - "System Overheated", + "TAKE CONTROL IMMEDIATELY", + "System Overheated", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "wrongGear": Alert( - "Take Control Immediately", - "Gear not D", + "TAKE CONTROL IMMEDIATELY", + "Gear not D", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInvalid": Alert( - "Take Control Immediately", - "Calibration Invalid: Reposition EON and Recalibrate", + "TAKE CONTROL IMMEDIATELY", + "Calibration Invalid: Reposition EON and Recalibrate", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInProgress": Alert( - "Take Control Immediately", + "TAKE CONTROL IMMEDIATELY", "Calibration in Progress", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "doorOpen": Alert( - "Take Control Immediately", - "Door Open", + "TAKE CONTROL IMMEDIATELY", + "Door Open", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "seatbeltNotLatched": Alert( - "Take Control Immediately", - "Seatbelt Unlatched", + "TAKE CONTROL IMMEDIATELY", + "Seatbelt Unlatched", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "espDisabled": Alert( - "Take Control Immediately", - "ESP Off", + "TAKE CONTROL IMMEDIATELY", + "ESP Off", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), # Cancellation alerts causing immediate disabling "radarCommIssue": Alert( - "Take Control Immediately", - "Radar Error: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Radar Error: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "radarFault": Alert( - "Take Control Immediately", - "Radar Error: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Radar Error: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "modelCommIssue": Alert( - "Take Control Immediately", - "Model Error: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Model Error: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsFailed": Alert( - "Take Control Immediately", - "Controls Failed", + "TAKE CONTROL IMMEDIATELY", + "Controls Failed", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsMismatch": Alert( - "Take Control Immediately", - "Controls Mismatch", + "TAKE CONTROL IMMEDIATELY", + "Controls Mismatch", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "commIssue": Alert( - "Take Control Immediately", - "CAN Error: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "CAN Error: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "steerUnavailable": Alert( - "Take Control Immediately", - "Steer Fault: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Steer Fault: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "brakeUnavailable": Alert( - "Take Control Immediately", - "Brake Fault: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Brake Fault: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "gasUnavailable": Alert( - "Take Control Immediately", - "Gas Fault: Restart the Car", + "TAKE CONTROL IMMEDIATELY", + "Gas Fault: Restart the Car", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "reverseGear": Alert( - "Take Control Immediately", - "Reverse Gear", + "TAKE CONTROL IMMEDIATELY", + "Reverse Gear", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "cruiseDisabled": Alert( - "Take Control Immediately", - "Cruise Is Off", + "TAKE CONTROL IMMEDIATELY", + "Cruise Is Off", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), # not loud cancellations (user is in control) "noTarget": Alert( "Comma Canceled", - "No Close Lead", - AlertStatus.normal, AlertSize.full, + "No close lead car", + AlertStatus.normal, AlertSize.mid, Priority.HIGH, None, "chimeDouble", .4, 2., 3.), "speedTooLow": Alert( "Comma Canceled", - "Speed Too Low", - AlertStatus.normal, AlertSize.full, + "Speed too low", + AlertStatus.normal, AlertSize.mid, Priority.HIGH, None, "chimeDouble", .4, 2., 3.), # Cancellation alerts causing non-entry "overheatNoEntry": Alert( - "Comma Unavailable", - "System Overheated", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "System overheated", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "wrongGearNoEntry": Alert( - "Comma Unavailable", - "Gear not D", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Gear not D", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInvalidNoEntry": Alert( - "Comma Unavailable", - "Calibration Invalid: Reposition EON and Recalibrate", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInProgressNoEntry": Alert( - "Comma Unavailable", - "Calibration in Progress", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Calibration in Progress", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "doorOpenNoEntry": Alert( - "Comma Unavailable", - "Door Open", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Door open", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "seatbeltNotLatchedNoEntry": Alert( - "Comma Unavailable", - "Seatbelt Unlatched", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Seatbelt unlatched", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), - + "espDisabledNoEntry": Alert( - "Comma Unavailable", - "ESP Off", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "ESP Off", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarCommIssueNoEntry": Alert( - "Comma Unavailable", - "Radar Error: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Radar Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarFaultNoEntry": Alert( - "Comma Unavailable", - "Radar Error: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Radar Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "modelCommIssueNoEntry": Alert( - "Comma Unavailable", - "Model Error: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Model Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "controlsFailedNoEntry": Alert( - "Comma Unavailable", - "Controls Failed", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Controls Failed", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "commIssueNoEntry": Alert( - "Comma Unavailable", - "CAN Error: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "CAN Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "steerUnavailableNoEntry": Alert( - "Comma Unavailable", - "Steer Fault: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Steer Fault: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeUnavailableNoEntry": Alert( - "Comma Unavailable", - "Brake Fault: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Brake Fault: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "gasUnavailableNoEntry": Alert( - "Comma Unavailable", - "Gas Error: Restart the Car", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Gas Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "reverseGearNoEntry": Alert( - "Comma Unavailable", - "Reverse Gear", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Reverse Gear", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "cruiseDisabledNoEntry": Alert( - "Comma Unavailable", - "Cruise is Off", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "Cruise is off", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "noTargetNoEntry": Alert( - "Comma Unavailable", - "No Close Lead", - AlertStatus.normal, AlertSize.full, + "Comma Unavailable", + "No close lead car", + AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), # permanent alerts to display on small UI upper box "steerUnavailablePermanent": Alert( - "STEER FAULT", - "RESTART THE CAR", + "STEER FAULT: Restart the car to engage", + "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, None, None, 0., 0., .2), "brakeUnavailablePermanent": Alert( - "BRAKE FAULT", - "RESTART THE CAR", + "BRAKE FAULT: Restart the car to engage", + "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, None, None, 0., 0., .2), "lowSpeedLockoutPermanent": Alert( - "CRUISE FAULT", - "RESTART THE CAR", + "CRUISE FAULT: Restart the car to engage", + "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, None, None, 0., 0., .2), } @@ -445,13 +445,14 @@ class AlertManager(object): enabled=enabled) self.activealerts.append(added_alert) - self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True) + # sort by priority first and then by start_time + self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True) # TODO: cycle through alerts? def process_alerts(self, cur_time): # first get rid of all the expired alerts - self.activealerts = [a for a in self.activealerts if a.start_time + + self.activealerts = [a for a in self.activealerts if a.start_time + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] ca = self.activealerts[0] if self.alertPresent() else None diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f98a0233..1fbd32c7 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -2,6 +2,20 @@ from common.numpy_fast import clip from cereal import car +class MPC_COST_LAT: + PATH = 1.0 + LANE = 3.0 + HEADING = 1.0 + STEER_RATE = 1.0 + + +class MPC_COST_LONG: + TTC = 5.0 + DISTANCE = 0.1 + ACCELERATION = 10.0 + JERK = 20.0 + + class EventTypes: ENABLE = 'enable' PRE_ENABLE = 'preEnable' diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d2737a04..e7271f7e 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,7 +1,7 @@ import math import numpy as np - from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT from selfdrive.controls.lib.lateral_mpc import libmpc_py from common.numpy_fast import interp from common.realtime import sec_since_boot @@ -34,7 +34,7 @@ class LatControl(object): def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc - self.libmpc.init(steer_rate_cost) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -74,7 +74,12 @@ class LatControl(object): l_poly, r_poly, p_poly, PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) - delta_desired = self.mpc_solution[0].delta[1] + # reset to current steer angle if not active or overriding + if active: + delta_desired = self.mpc_solution[0].delta[1] + else: + delta_desired = math.radians(angle_steers - angle_offset) / VM.CP.steerRatio + self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset) @@ -85,7 +90,7 @@ class LatControl(object): nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if nans: - self.libmpc.init(VM.CP.steerRateCost) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio if t > self.last_cloudlog_t + 5.0: @@ -108,4 +113,4 @@ class LatControl(object): output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego) self.sat_flag = self.pid.saturated - return output_steer + return output_steer, float(self.angle_steers_des) diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 53438a11..9c6dd31e 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -4,7 +4,6 @@ #define deg2rad(d) (d/180.0*PI) const int controlHorizon = 50; -const double samplingTime = 0.05; // 20 Hz using namespace std; diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 33bbe189..ab5be2f6 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -1,12 +1,11 @@ import os -import sys import subprocess from cffi import FFI mpc_dir = os.path.dirname(os.path.abspath(__file__)) libmpc_fn = os.path.join(mpc_dir, "libcommampc.so") -subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], cwd=mpc_dir) ffi = FFI() ffi.cdef(""" @@ -21,7 +20,7 @@ typedef struct { double delta[20]; } log_t; -void init(double steer_rate_cost); +void init(double pathCost, double laneCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, double l_poly[4], double r_poly[4], double p_poly[4], double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width); diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c index 3c082319..0cae4079 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -28,9 +28,10 @@ typedef struct { double delta[N]; } log_t; -void init(double steerRateCost){ +void init(double pathCost, double laneCost, double headingCost, double steerRateCost){ acado_initializeSolver(); int i; + const int STEP_MULTIPLIER = 3; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; @@ -46,18 +47,18 @@ void init(double steerRateCost){ for (i = 0; i < N; i++) { int f = 1; if (i > 4){ - f = 3; + f = STEP_MULTIPLIER; } - 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 + 0] = pathCost * f; + acadoVariables.W[25 * i + 6] = laneCost * f; + acadoVariables.W[25 * i + 12] = laneCost * f; + acadoVariables.W[25 * i + 18] = headingCost * 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; + acadoVariables.WN[0] = pathCost * STEP_MULTIPLIER; + acadoVariables.WN[5] = laneCost * STEP_MULTIPLIER; + acadoVariables.WN[10] = laneCost * STEP_MULTIPLIER; + acadoVariables.WN[15] = headingCost * STEP_MULTIPLIER; } int run_mpc(state_t * x0, log_t * solution, diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index 264a2ae0..d21dc585 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -1,7 +1,6 @@ #include const int controlHorizon = 50; -const double samplingTime = 0.2; using namespace std; diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 8020b63c..8919bd42 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -1,11 +1,10 @@ import os -import sys import subprocess from cffi import FFI mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], cwd=mpc_dir) def _get_libmpc(mpc_id): @@ -28,7 +27,7 @@ def _get_libmpc(mpc_id): double a_l[20]; } log_t; - void init(); + void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); int run_mpc(state_t * x0, log_t * solution, double l); diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/mpc.c index 4e2c13c1..e56eca15 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc.c @@ -31,9 +31,10 @@ typedef struct { double a_l[N]; } log_t; -void init(){ +void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ acado_initializeSolver(); int i; + const int STEP_MULTIPLIER = 3; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; @@ -50,16 +51,16 @@ void init(){ for (i = 0; i < N; i++) { int f = 1; if (i > 4){ - f = 3; + f = STEP_MULTIPLIER; } - 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.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration + acadoVariables.W[16 * i + 15] = jerkCost * 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 + acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 219ba42e..6814edd3 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -12,7 +12,7 @@ import selfdrive.messaging as messaging from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.services import service_list -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.speed_smoother import speed_smoother @@ -34,8 +34,8 @@ _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits -_A_CRUISE_MAX_V = [1., 1., .8, .5, .3] -_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .3] +_A_CRUISE_MAX_V = [1.1, 1.1, .8, .5, .3] +_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 1.2, .7, .3] _A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] # Lookup table for turns @@ -172,7 +172,8 @@ class LongitudinalMpc(object): def setup_mpc(self): ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) - self.libmpc.init() + self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, + MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.mpc_solution = ffi.new("log_t *") self.cur_state = ffi.new("state_t *") @@ -245,7 +246,8 @@ class LongitudinalMpc(object): cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % ( self.mpc_id, backwards, crashing, nans)) - self.libmpc.init() + self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, + MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.cur_state[0].v_ego = CS.vEgo self.cur_state[0].a_ego = 0.0 self.v_mpc = CS.vEgo diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 0fa226bc..ca41da7c 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -213,7 +213,7 @@ class Cluster(object): lead.fcw = self.is_potential_fcw() def __str__(self): - ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aRel, self.dPath) + ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath) if self.stationary: ret += " stationary" if self.vision: diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 4ffe15cb..bf7d14b6 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -260,12 +260,13 @@ def radard_thread(gctx=None): for cnt, ids in enumerate(tracks.keys()): if DEBUG: - print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \ + print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, - tracks[ids].stationary) + tracks[ids].stationary, + tracks[ids].measured) dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 18c27ec1..20003ee9 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -1,6 +1,7 @@ """Install exception handler for process crash.""" import os import sys +from selfdrive.version import version, dirty from selfdrive.swaglog import cloudlog @@ -16,9 +17,8 @@ if os.getenv("NOLOG") or os.getenv("NOCRASH"): else: from raven import Client from raven.transport.http import HTTPTransport - client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', - install_sys_hook=False, transport=HTTPTransport) + install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) def capture_exception(*args, **kwargs): client.captureException(*args, **kwargs) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index e1713d34..89dd43fa 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -4,10 +4,15 @@ import argparse import zmq import json from hexdump import hexdump +from threading import Thread +from cereal import log import selfdrive.messaging as messaging from selfdrive.services import service_list +def run_server(socketio): + socketio.run(app, host='0.0.0.0', port=4000) + if __name__ == "__main__": context = zmq.Context() poller = zmq.Poller() @@ -17,33 +22,63 @@ if __name__ == "__main__": parser.add_argument('--raw', action='store_true') parser.add_argument('--json', action='store_true') parser.add_argument('--dump-json', action='store_true') + parser.add_argument('--no-print', action='store_true') + parser.add_argument('--proxy', action='store_true', help='republish on localhost') + parser.add_argument('--map', action='store_true') parser.add_argument('--addr', default='127.0.0.1') parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() + republish_socks = {} + for m in args.socket if len(args.socket) > 0 else service_list: if m in service_list: - messaging.sub_sock(context, service_list[m].port, poller, addr=args.addr) + port = service_list[m].port elif m.isdigit(): - messaging.sub_sock(context, int(m), poller, addr=args.addr) + port = int(m) else: print("service not found") exit(-1) + sock = messaging.sub_sock(context, port, poller, addr=args.addr) + if args.proxy: + republish_socks[sock] = messaging.pub_sock(context, port) + + if args.map: + from flask.ext.socketio import SocketIO + from flask import Flask + app = Flask(__name__) + socketio = SocketIO(app, async_mode='threading') + server_thread = Thread(target=run_server, args=(socketio,)) + server_thread.daemon = True + server_thread.start() + print 'server running' while 1: polld = poller.poll(timeout=1000) for sock, mode in polld: if mode != zmq.POLLIN: continue - if args.pipe: - sys.stdout.write(sock.recv()) - sys.stdout.flush() - elif args.raw: - hexdump(sock.recv()) - elif args.json: - print(json.loads(sock.recv())) - elif args.dump_json: - print json.dumps(messaging.recv_one(sock).to_dict()) - else: - print messaging.recv_one(sock) + msg = sock.recv() + evt = log.Event.from_bytes(msg) + if sock in republish_socks: + republish_socks[sock].send(msg) + if args.map and evt.which() == 'liveLocation': + print 'send loc' + socketio.emit('location', { + 'lat': evt.liveLocation.lat, + 'lon': evt.liveLocation.lon, + 'alt': evt.liveLocation.alt, + }) + if not args.no_print: + if args.pipe: + sys.stdout.write(msg) + sys.stdout.flush() + elif args.raw: + hexdump(msg) + elif args.json: + print(json.loads(msg)) + elif args.dump_json: + print json.dumps(evt.to_dict()) + else: + print evt diff --git a/selfdrive/locationd/__init__.py b/selfdrive/locationd/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/selfdrive/locationd/ephemeris.py b/selfdrive/locationd/ephemeris.py new file mode 100644 index 00000000..0fbedf52 --- /dev/null +++ b/selfdrive/locationd/ephemeris.py @@ -0,0 +1,140 @@ +def GET_FIELD_U(w, nb, pos): + return (((w) >> (pos)) & ((1 << (nb)) - 1)) + + +def twos_complement(v, nb): + sign = v >> (nb - 1) + value = v + if sign != 0: + value = value - (1 << nb) + return value + + +def GET_FIELD_S(w, nb, pos): + v = GET_FIELD_U(w, nb, pos) + return twos_complement(v, nb) + + +def extract_uint8(v, b): + return (v >> (8 * (3 - b))) & 255 + +def extract_int8(v, b): + value = extract_uint8(v, b) + if value > 127: + value -= 256 + return value + +class EphemerisData: + '''container for parsing a AID_EPH message + Thanks to Sylvain Munaut + http://cgit.osmocom.org/cgit/osmocom-lcs/tree/gps.c +on of this parser + + See IS-GPS-200F.pdf Table 20-III for the field meanings, scaling factors and + field widths + ''' + + def __init__(self, svId, subframes): + from math import pow + self.svId = svId + week_no = GET_FIELD_U(subframes[1][2+0], 10, 20) + # code_on_l2 = GET_FIELD_U(subframes[1][0], 2, 12) + # sv_ura = GET_FIELD_U(subframes[1][0], 4, 8) + # sv_health = GET_FIELD_U(subframes[1][0], 6, 2) + # l2_p_flag = GET_FIELD_U(subframes[1][1], 1, 23) + t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6) + iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( + subframes[1][2+5], 8, 22) + + t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6) + a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22) + a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6) + a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8) + + c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6) + delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14) + m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+2], 24, 6) + c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14) + e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6) + c_us = GET_FIELD_S(subframes[2][2+5], 16, 14) + a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+6], 24, 6) + t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14) + # fit_flag = GET_FIELD_U(subframes[2][7], 1, 7) + + c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14) + omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+1], 24, 6) + c_is = GET_FIELD_S(subframes[3][2+2], 16, 14) + i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+3], 24, 6) + c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14) + w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6) + omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6) + idot = GET_FIELD_S(subframes[3][2+7], 14, 8) + + self._rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6) + self._rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6) + self._rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6) + self._rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14) + self.aodo = GET_FIELD_U(subframes[2][2+7], 5, 8) + + # Definition of Pi used in the GPS coordinate system + gpsPi = 3.1415926535898 + + # now form variables in radians, meters and seconds etc + self.Tgd = t_gd * pow(2, -31) + self.A = pow(a_powhalf * pow(2, -19), 2.0) + self.cic = c_ic * pow(2, -29) + self.cis = c_is * pow(2, -29) + self.crc = c_rc * pow(2, -5) + self.crs = c_rs * pow(2, -5) + self.cuc = c_uc * pow(2, -29) + self.cus = c_us * pow(2, -29) + self.deltaN = delta_n * pow(2, -43) * gpsPi + self.ecc = e * pow(2, -33) + self.i0 = i_0 * pow(2, -31) * gpsPi + self.idot = idot * pow(2, -43) * gpsPi + self.M0 = m_0 * pow(2, -31) * gpsPi + self.omega = w * pow(2, -31) * gpsPi + self.omega_dot = omega_dot * pow(2, -43) * gpsPi + self.omega0 = omega_0 * pow(2, -31) * gpsPi + self.toe = t_oe * pow(2, 4) + + # clock correction information + self.toc = t_oc * pow(2, 4) + self.gpsWeek = week_no + self.af0 = a_f0 * pow(2, -31) + self.af1 = a_f1 * pow(2, -43) + self.af2 = a_f2 * pow(2, -55) + + iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22) + iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22) + self.valid = (iode1 == iode2) and (iode1 == (iodc & 0xff)) + self.iode = iode1 + + if GET_FIELD_U(subframes[4][2+0], 2, 28) != 1: + raise RuntimeError("subframe 4 not of type 1") + if GET_FIELD_U(subframes[5][2+0], 2, 28) != 1: + raise RuntimeError("subframe 5 not of type 1") + print 'page :', GET_FIELD_U(subframes[4][2+0], 6, 22) + if GET_FIELD_U(subframes[4][2+0], 6, 22) == 56: + a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30) + a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27) + a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24) + a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24) + b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11) + b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14) + b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16) + b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16) + + self.ionoAlpha = [a0, a1, a2, a3] + self.ionoBeta = [b0, b1, b2, b3] + self.ionoCoeffsValid = True + else: + self.ionoAlpha = [] + self.ionoBeta = [] + self.ionoCoeffsValid = False + + diff --git a/selfdrive/locationd/locationd_dummy.py b/selfdrive/locationd/locationd_dummy.py new file mode 100755 index 00000000..2c44932f --- /dev/null +++ b/selfdrive/locationd/locationd_dummy.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python +import zmq +from copy import copy +from selfdrive import messaging +from selfdrive.services import service_list +from cereal import log + +from common.transformations.coordinates import geodetic2ecef + +def main(gctx=None): + context = zmq.Context() + poller = zmq.Poller() + gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, poller) + gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller) + app_sock = messaging.sub_sock(context, service_list['applanixLocation'].port, poller) + loc_sock = messaging.pub_sock(context, service_list['liveLocation'].port) + + last_ext, last_gps, last_app = -1, -1, -1 + # 5 sec + max_gap = 5*1e9 + preferred_type = None + + while 1: + for sock, event in poller.poll(500): + if sock is app_sock: + msg = messaging.recv_one(sock) + last_app = msg.logMonoTime + this_type = 'app' + if sock is gps_sock: + msg = messaging.recv_one(sock) + gps_pkt = msg.gpsLocation + last_gps = msg.logMonoTime + this_type = 'gps' + if sock is gps_ext_sock: + msg = messaging.recv_one(sock) + gps_pkt = msg.gpsLocationExternal + last_ext = msg.logMonoTime + this_type = 'ext' + + last = max(last_gps, last_ext, last_app) + + if last_app > last - max_gap: + new_preferred_type = 'app' + elif last_ext > last - max_gap: + new_preferred_type = 'ext' + else: + new_preferred_type = 'gps' + + if preferred_type != new_preferred_type: + print "switching from %s to %s" % (preferred_type, new_preferred_type) + preferred_type = new_preferred_type + + if this_type == preferred_type: + new_msg = messaging.new_message() + if this_type == 'app': + # straight proxy the applanix + new_msg.init('liveLocation') + new_msg.liveLocation = copy(msg.applanixLocation) + else: + new_msg.logMonoTime = msg.logMonoTime + new_msg.init('liveLocation') + pkt = new_msg.liveLocation + pkt.lat = gps_pkt.latitude + pkt.lon = gps_pkt.longitude + pkt.alt = gps_pkt.altitude + pkt.speed = gps_pkt.speed + pkt.heading = gps_pkt.bearing + pkt.positionECEF = [float(x) for x in geodetic2ecef([pkt.lat, pkt.lon, pkt.alt])] + pkt.source = log.LiveLocationData.SensorSource.dummy + loc_sock.send(new_msg.to_bytes()) + +if __name__ == '__main__': + main() diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/ublox.py new file mode 100644 index 00000000..228dfc84 --- /dev/null +++ b/selfdrive/locationd/ublox.py @@ -0,0 +1,995 @@ +#!/usr/bin/env python +''' +UBlox binary protocol handling + +Copyright Andrew Tridgell, October 2012 +Released under GNU GPL version 3 or later + +WARNING: This code has originally intended for +ublox version 7, it has been adapted to work +for ublox version 8, not all functions may work. +''' + +import struct +import time, os + +# protocol constants +PREAMBLE1 = 0xb5 +PREAMBLE2 = 0x62 + +# message classes +CLASS_NAV = 0x01 +CLASS_RXM = 0x02 +CLASS_INF = 0x04 +CLASS_ACK = 0x05 +CLASS_CFG = 0x06 +CLASS_MON = 0x0A +CLASS_AID = 0x0B +CLASS_TIM = 0x0D +CLASS_ESF = 0x10 + +# ACK messages +MSG_ACK_NACK = 0x00 +MSG_ACK_ACK = 0x01 + +# NAV messages +MSG_NAV_POSECEF = 0x1 +MSG_NAV_POSLLH = 0x2 +MSG_NAV_STATUS = 0x3 +MSG_NAV_DOP = 0x4 +MSG_NAV_SOL = 0x6 +MSG_NAV_PVT = 0x7 +MSG_NAV_POSUTM = 0x8 +MSG_NAV_VELNED = 0x12 +MSG_NAV_VELECEF = 0x11 +MSG_NAV_TIMEGPS = 0x20 +MSG_NAV_TIMEUTC = 0x21 +MSG_NAV_CLOCK = 0x22 +MSG_NAV_SVINFO = 0x30 +MSG_NAV_AOPSTATUS = 0x60 +MSG_NAV_DGPS = 0x31 +MSG_NAV_DOP = 0x04 +MSG_NAV_EKFSTATUS = 0x40 +MSG_NAV_SBAS = 0x32 +MSG_NAV_SOL = 0x06 + +# RXM messages +MSG_RXM_RAW = 0x15 +MSG_RXM_SFRB = 0x11 +MSG_RXM_SFRBX = 0x13 +MSG_RXM_SVSI = 0x20 +MSG_RXM_EPH = 0x31 +MSG_RXM_ALM = 0x30 +MSG_RXM_PMREQ = 0x41 + +# AID messages +MSG_AID_ALM = 0x30 +MSG_AID_EPH = 0x31 +MSG_AID_ALPSRV = 0x32 +MSG_AID_AOP = 0x33 +MSG_AID_DATA = 0x10 +MSG_AID_ALP = 0x50 +MSG_AID_DATA = 0x10 +MSG_AID_HUI = 0x02 +MSG_AID_INI = 0x01 +MSG_AID_REQ = 0x00 + +# CFG messages +MSG_CFG_PRT = 0x00 +MSG_CFG_ANT = 0x13 +MSG_CFG_DAT = 0x06 +MSG_CFG_EKF = 0x12 +MSG_CFG_ESFGWT = 0x29 +MSG_CFG_CFG = 0x09 +MSG_CFG_USB = 0x1b +MSG_CFG_RATE = 0x08 +MSG_CFG_SET_RATE = 0x01 +MSG_CFG_NAV5 = 0x24 +MSG_CFG_FXN = 0x0E +MSG_CFG_INF = 0x02 +MSG_CFG_ITFM = 0x39 +MSG_CFG_MSG = 0x01 +MSG_CFG_NAVX5 = 0x23 +MSG_CFG_NMEA = 0x17 +MSG_CFG_NVS = 0x22 +MSG_CFG_PM2 = 0x3B +MSG_CFG_PM = 0x32 +MSG_CFG_RINV = 0x34 +MSG_CFG_RST = 0x04 +MSG_CFG_RXM = 0x11 +MSG_CFG_SBAS = 0x16 +MSG_CFG_TMODE2 = 0x3D +MSG_CFG_TMODE = 0x1D +MSG_CFG_TPS = 0x31 +MSG_CFG_TP = 0x07 +MSG_CFG_GNSS = 0x3E +MSG_CFG_ODO = 0x1E + +# ESF messages +MSG_ESF_MEAS = 0x02 +MSG_ESF_STATUS = 0x10 + +# INF messages +MSG_INF_DEBUG = 0x04 +MSG_INF_ERROR = 0x00 +MSG_INF_NOTICE = 0x02 +MSG_INF_TEST = 0x03 +MSG_INF_WARNING = 0x01 + +# MON messages +MSG_MON_SCHD = 0x01 +MSG_MON_HW = 0x09 +MSG_MON_HW2 = 0x0B +MSG_MON_IO = 0x02 +MSG_MON_MSGPP = 0x06 +MSG_MON_RXBUF = 0x07 +MSG_MON_RXR = 0x21 +MSG_MON_TXBUF = 0x08 +MSG_MON_VER = 0x04 + +# TIM messages +MSG_TIM_TP = 0x01 +MSG_TIM_TM2 = 0x03 +MSG_TIM_SVIN = 0x04 +MSG_TIM_VRFY = 0x06 + +# port IDs +PORT_DDC = 0 +PORT_SERIAL1 = 1 +PORT_SERIAL2 = 2 +PORT_USB = 3 +PORT_SPI = 4 + +# dynamic models +DYNAMIC_MODEL_PORTABLE = 0 +DYNAMIC_MODEL_STATIONARY = 2 +DYNAMIC_MODEL_PEDESTRIAN = 3 +DYNAMIC_MODEL_AUTOMOTIVE = 4 +DYNAMIC_MODEL_SEA = 5 +DYNAMIC_MODEL_AIRBORNE1G = 6 +DYNAMIC_MODEL_AIRBORNE2G = 7 +DYNAMIC_MODEL_AIRBORNE4G = 8 + +#reset items +RESET_HOT = 0 +RESET_WARM = 1 +RESET_COLD = 0xFFFF + +RESET_HW = 0 +RESET_SW = 1 +RESET_SW_GPS = 2 +RESET_HW_GRACEFUL = 4 +RESET_GPS_STOP = 8 +RESET_GPS_START = 9 + + +class UBloxError(Exception): + '''Ublox error class''' + + def __init__(self, msg): + Exception.__init__(self, msg) + self.message = msg + + +class UBloxAttrDict(dict): + '''allow dictionary members as attributes''' + + def __init__(self): + dict.__init__(self) + + def __getattr__(self, name): + try: + return self.__getitem__(name) + except KeyError: + raise AttributeError(name) + + def __setattr__(self, name, value): + if self.__dict__.has_key(name): + # allow set on normal attributes + dict.__setattr__(self, name, value) + else: + self.__setitem__(name, value) + + +def ArrayParse(field): + '''parse an array descriptor''' + arridx = field.find('[') + if arridx == -1: + return (field, -1) + alen = int(field[arridx + 1:-1]) + fieldname = field[:arridx] + return (fieldname, alen) + + +class UBloxDescriptor: + '''class used to describe the layout of a UBlox message''' + + def __init__(self, + name, + msg_format, + fields=[], + count_field=None, + format2=None, + fields2=None): + self.name = name + self.msg_format = msg_format + self.fields = fields + self.count_field = count_field + self.format2 = format2 + self.fields2 = fields2 + + def unpack(self, msg): + '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg''' + msg._fields = {} + + # unpack main message blocks. A comm + formats = self.msg_format.split(',') + buf = msg._buf[6:-2] + count = 0 + msg._recs = [] + fields = self.fields[:] + + for fmt in formats: + size1 = struct.calcsize(fmt) + if size1 > len(buf): + raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf))) + f1 = list(struct.unpack(fmt, buf[:size1])) + i = 0 + while i < len(f1): + field = fields.pop(0) + (fieldname, alen) = ArrayParse(field) + if alen == -1: + msg._fields[fieldname] = f1[i] + if self.count_field == fieldname: + count = int(f1[i]) + i += 1 + else: + msg._fields[fieldname] = [0] * alen + for a in range(alen): + msg._fields[fieldname][a] = f1[i] + i += 1 + buf = buf[size1:] + if len(buf) == 0: + break + + if self.count_field == '_remaining': + count = len(buf) / struct.calcsize(self.format2) + + if count == 0: + msg._unpacked = True + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + return + + size2 = struct.calcsize(self.format2) + for c in range(count): + r = UBloxAttrDict() + if size2 > len(buf): + raise UBloxError("INVALID_SIZE=%u, " % len(buf)) + f2 = list(struct.unpack(self.format2, buf[:size2])) + for i in range(len(self.fields2)): + r[self.fields2[i]] = f2[i] + buf = buf[size2:] + msg._recs.append(r) + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + msg._unpacked = True + + def pack(self, msg, msg_class=None, msg_id=None): + '''pack a UBloxMessage from the .fields and ._recs attributes in msg''' + f1 = [] + if msg_class is None: + msg_class = msg.msg_class() + if msg_id is None: + msg_id = msg.msg_id() + msg._buf = '' + + fields = self.fields[:] + for f in fields: + (fieldname, alen) = ArrayParse(f) + if not fieldname in msg._fields: + break + if alen == -1: + f1.append(msg._fields[fieldname]) + else: + for a in range(alen): + f1.append(msg._fields[fieldname][a]) + try: + # try full length message + fmt = self.msg_format.replace(',', '') + msg._buf = struct.pack(fmt, *tuple(f1)) + except Exception: + # try without optional part + fmt = self.msg_format.split(',')[0] + msg._buf = struct.pack(fmt, *tuple(f1)) + + length = len(msg._buf) + if msg._recs: + length += len(msg._recs) * struct.calcsize(self.format2) + header = struct.pack('= level: + print(msg) + + def unpack(self): + '''unpack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + msg_types[type].unpack(self) + return self._fields, self._recs + + def pack(self): + '''pack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s' % str(type)) + msg_types[type].pack(self) + + def name(self): + '''return the short string name for a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + return msg_types[type].name + + def msg_class(self): + '''return the message class''' + return ord(self._buf[2]) + + def msg_id(self): + '''return the message id within the class''' + return ord(self._buf[3]) + + def msg_type(self): + '''return the message type tuple (class, id)''' + return (self.msg_class(), self.msg_id()) + + def msg_length(self): + '''return the payload length''' + (payload_length, ) = struct.unpack(' 0 and ord(self._buf[0]) != PREAMBLE1: + return False + if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2: + self.debug(1, "bad pre2") + return False + if self.needed_bytes() == 0 and not self.valid(): + if len(self._buf) > 8: + self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf), + self.needed_bytes())) + else: + self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes())) + return False + return True + + def add(self, bytes): + '''add some bytes to a message''' + self._buf += bytes + while not self.valid_so_far() and len(self._buf) > 0: + '''handle corrupted streams''' + self._buf = self._buf[1:] + if self.needed_bytes() < 0: + self._buf = "" + + def checksum(self, data=None): + '''return a checksum tuple for a message''' + if data is None: + data = self._buf[2:-2] + #cs = 0 + ck_a = 0 + ck_b = 0 + for i in data: + ck_a = (ck_a + ord(i)) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return (ck_a, ck_b) + + def valid_checksum(self): + '''check if the checksum is OK''' + (ck_a, ck_b) = self.checksum() + #d = self._buf[2:-2] + (ck_a2, ck_b2) = struct.unpack('= 8 and self.needed_bytes() == 0 and self.valid_checksum() + + +class UBlox: + '''main UBlox control class. + + port can be a file (for reading only) or a serial device + ''' + + def __init__(self, port, baudrate=115200, timeout=0, panda=False, grey=False): + + self.serial_device = port + self.baudrate = baudrate + self.use_sendrecv = False + self.read_only = False + self.debug_level = 0 + + if panda: + from panda import Panda, PandaSerial + + self.panda = Panda() + + # resetting U-Blox module + self.panda.set_esp_power(0) + time.sleep(0.1) + self.panda.set_esp_power(1) + time.sleep(0.5) + + # can't set above 9600 now... + self.baudrate = 9600 + self.dev = PandaSerial(self.panda, 1, self.baudrate) + + self.baudrate = 460800 + print "upping baud:",self.baudrate + self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate) + time.sleep(0.1) + + self.dev = PandaSerial(self.panda, 1, self.baudrate) + elif grey: + import zmq + from selfdrive.services import service_list + import selfdrive.messaging as messaging + + class BoarddSerial(object): + def __init__(self): + context = zmq.Context() + self.ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port) + self.buf = "" + + def read(self, n): + for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n): + self.buf += msg.ubloxRaw + ret = self.buf[:n] + self.buf = self.buf[n:] + return ret + + + def write(self, dat): + pass + + self.dev = BoarddSerial() + else: + if self.serial_device.startswith("tcp:"): + import socket + a = self.serial_device.split(':') + destination_addr = (a[1], int(a[2])) + self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.dev.connect(destination_addr) + self.dev.setblocking(1) + self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) + self.use_sendrecv = True + elif os.path.isfile(self.serial_device): + self.read_only = True + self.dev = open(self.serial_device, mode='rb') + else: + import serial + self.dev = serial.Serial( + self.serial_device, + baudrate=self.baudrate, + dsrdtr=False, + rtscts=False, + xonxoff=False, + timeout=timeout) + + self.logfile = None + self.log = None + self.preferred_dynamic_model = None + self.preferred_usePPP = None + self.preferred_dgps_timeout = None + + def close(self): + '''close the device''' + self.dev.close() + self.dev = None + + def set_debug(self, debug_level): + '''set debug level''' + self.debug_level = debug_level + + def debug(self, level, msg): + '''write a debug message''' + if self.debug_level >= level: + print(msg) + + def set_logfile(self, logfile, append=False): + '''setup logging to a file''' + if self.log is not None: + self.log.close() + self.log = None + self.logfile = logfile + if self.logfile is not None: + if append: + mode = 'ab' + else: + mode = 'wb' + self.log = open(self.logfile, mode=mode) + + def set_preferred_dynamic_model(self, model): + '''set the preferred dynamic model for receiver''' + self.preferred_dynamic_model = model + if model is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_dgps_timeout(self, timeout): + '''set the preferred DGPS timeout for receiver''' + self.preferred_dgps_timeout = timeout + if timeout is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_usePPP(self, usePPP): + '''set the preferred usePPP setting for the receiver''' + if usePPP is None: + self.preferred_usePPP = None + return + self.preferred_usePPP = int(usePPP) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def nmea_checksum(self, msg): + d = msg[1:] + cs = 0 + for i in d: + cs ^= ord(i) + return cs + + def write(self, buf): + '''write some bytes''' + if not self.read_only: + if self.use_sendrecv: + return self.dev.send(buf) + return self.dev.write(buf) + + def read(self, n): + '''read some bytes''' + if self.use_sendrecv: + import socket + try: + return self.dev.recv(n) + except socket.error: + return '' + return self.dev.read(n) + + def send_nmea(self, msg): + if not self.read_only: + s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n" + self.write(s) + + def set_binary(self): + '''put a UBlox into binary mode using a NMEA string''' + if not self.read_only: + print("try set binary at %u" % self.baudrate) + self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate) + + def disable_nmea(self): + ''' stop sending all types of nmea messages ''' + self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0") + self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0") + + def seek_percent(self, pct): + '''seek to the given percentage of a file''' + self.dev.seek(0, 2) + filesize = self.dev.tell() + self.dev.seek(pct * 0.01 * filesize) + + def special_handling(self, msg): + '''handle automatic configuration changes''' + if msg.name() == 'CFG_NAV5': + msg.unpack() + sendit = False + pollit = False + if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model: + msg.dynModel = self.preferred_dynamic_model + sendit = True + pollit = True + if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout: + msg.dgpsTimeOut = self.preferred_dgps_timeout + self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut) + sendit = True + # we don't re-poll for this one, as some receivers refuse to set it + if sendit: + msg.pack() + self.send(msg) + if pollit: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None: + msg.unpack() + if msg.usePPP != self.preferred_usePPP: + msg.usePPP = self.preferred_usePPP + msg.mask = 1 << 13 + msg.pack() + self.send(msg) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def receive_message(self, ignore_eof=False): + '''blocking receive of one ublox message''' + msg = UBloxMessage() + while True: + n = msg.needed_bytes() + b = self.read(n) + if not b: + if ignore_eof: + time.sleep(0.01) + continue + if len(msg._buf) > 0: + self.debug(1, "dropping %d bytes" % len(msg._buf)) + return None + msg.add(b) + if self.log is not None: + self.log.write(b) + self.log.flush() + if msg.valid(): + self.special_handling(msg) + return msg + + def receive_message_noerror(self, ignore_eof=False): + '''blocking receive of one ublox message, ignoring errors''' + try: + return self.receive_message(ignore_eof=ignore_eof) + except UBloxError as e: + print(e) + return None + except OSError as e: + # Occasionally we get hit with 'resource temporarily unavailable' + # messages here on the serial device, catch them too. + print(e) + return None + + def send(self, msg): + '''send a preformatted ublox message''' + if not msg.valid(): + self.debug(1, "invalid send") + return + if not self.read_only: + self.write(msg._buf) + + def send_message(self, msg_class, msg_id, payload): + '''send a ublox message with class, id and payload''' + msg = UBloxMessage() + msg._buf = struct.pack(' /sys/module/dwc3_msm/parameters/otg_switch") + from smbus2 import SMBus bus = SMBus(7, force=True) bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable @@ -300,6 +298,7 @@ def set_eon_fan(val): if not EON: return + from smbus2 import SMBus if last_eon_fan_val is None or last_eon_fan_val != val: bus = SMBus(7, force=True) bus.write_byte_data(0x21, 0x04, 0x2) @@ -391,7 +390,6 @@ def manager_thread(): params = Params() - passive = params.get("Passive") == "1" passive_starter = LocationStarter() started_ts = None @@ -400,6 +398,7 @@ def manager_thread(): fan_speed = 0 ignition_seen = False battery_was_high = False + panda_seen = False health_sock.RCVTIMEO = 1500 @@ -455,13 +454,20 @@ def manager_thread(): ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition + # add voltage check for ignition + if not ignition_seen and td is not None and td.health.voltage > 13500: + ignition = True + do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" should_start = ignition - # start on gps in passive mode - if passive and not ignition_seen: + # have we seen a panda? + panda_seen = panda_seen or td is not None + + # start on gps if we have no connection to a panda + if not panda_seen: should_start = should_start or passive_starter.update(started_ts, location) # with 2% left, we killall, otherwise the phone will take a long time to boot @@ -509,6 +515,7 @@ def manager_thread(): running=running.keys(), count=count, health=(td.to_dict() if td else None), + location=(location.to_dict() if location else None), thermal=msg.to_dict()) if do_uninstall: @@ -641,7 +648,12 @@ def main(): if params.get("IsUploadVideoOverCellularEnabled") is None: params.put("IsUploadVideoOverCellularEnabled", "1") - params.put("Passive", "1" if os.getenv("PASSIVE") else "0") + # is this chffrplus? + if os.getenv("PASSIVE") is not None: + params.put("Passive", str(int(os.getenv("PASSIVE")))) + + if params.get("Passive") is None: + raise Exception("Passive must be set to continue") # put something on screen while we set things up if os.getenv("PREPAREONLY") is not None: diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index c4fd9158..4321c710 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 75fd6cc6..c470f35c 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 0d726314..897df442 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -55,13 +55,22 @@ gpsPlannerPoints: [8043, true] gpsPlannerPlan: [8044, true] applanixRaw: [8046, true] orbLocation: [8047, true] -trafficSigns: [8048, true] +trafficEvents: [8048, true] liveLocationTiming: [8049, true] orbslamCorrection: [8050, true] liveLocationCorrected: [8051, true] +orbObservation: [8052, true] +applanixLocation: [8053, true] +liveLocationKalman: [8054, true] +uiNavigationEvent: [8055, true] +orbOdometry: [8057, true] +orbFeatures: [8058, true] +orbKeyFrame: [8059, true] +uiLayoutState: [8060, true] testModel: [8040, false] testLiveLocation: [8045, false] +testJoystick: [8056, false] # manager -- base process to manage starting and stopping of all others diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 1c97cea4..ca20ce9a 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -170,7 +170,7 @@ class Plant(object): fcw = True if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: - brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] + brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248 else: brake = 0.0 diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 430b31d8..bac43a0a 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -7,7 +7,7 @@ import datetime from raven import Client from raven.transport.http import HTTPTransport -from selfdrive.version import version +from selfdrive.version import version, dirty from selfdrive.swaglog import cloudlog def get_tombstones(): @@ -69,7 +69,7 @@ def main(gctx): initial_tombstones = set(get_tombstones()) client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', - install_sys_hook=False, transport=HTTPTransport) + install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) while True: now_tombstones = set(get_tombstones()) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 99d351db..b6d1c261 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -44,13 +44,28 @@ #define STATUS_ALERT 4 #define STATUS_MAX 5 +#define ALERTSIZE_NONE 0 +#define ALERTSIZE_SMALL 1 +#define ALERTSIZE_MID 2 +#define ALERTSIZE_FULL 3 + #define UI_BUF_COUNT 4 - -const int box_x = 330; -const int box_y = 30; -const int box_width = 1560; -const int box_height = 1020; +const int vwp_w = 1920; +const int vwp_h = 1080; +const int nav_w = 0; +const int nav_ww= 0; +const int sbr_w = 300; +const int bdr_s = 30; +const int box_x = sbr_w+bdr_s; +const int box_y = bdr_s; +const int box_w = vwp_w-sbr_w-(bdr_s*2); +const int box_h = vwp_h-(bdr_s*2); +const int viz_x = box_x+nav_w; +const int viz_y = box_y; +const int viz_w = box_w-nav_w; +const int viz_h = box_h; +const int viz_header_h = 420; const uint8_t bg_colors[][4] = { [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, @@ -61,11 +76,18 @@ const uint8_t bg_colors[][4] = { }; const uint8_t alert_colors[][4] = { - [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0x80}, - [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0x80}, - [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0x80}, - [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0x80}, - [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0x80}, + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xf1}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, +}; + +const int alert_sizes[] = { + [ALERTSIZE_NONE] = 0, + [ALERTSIZE_SMALL] = 241, + [ALERTSIZE_MID] = 390, + [ALERTSIZE_FULL] = vwp_h, }; typedef struct UIScene { @@ -91,6 +113,15 @@ typedef struct UIScene { float curvature; int engaged; + bool uilayout_sidebarcollapsed; + bool uilayout_mapenabled; + // responsive sizes for sidebar + int ui_viz_rx; + int ui_viz_rw; + // responsize offset for frame + projections + int ui_frame_offset; + int ui_world_offset; + int lead_status; float lead_d_rel, lead_y_rel, lead_v_rel; @@ -110,7 +141,6 @@ typedef struct UIScene { int cal_status; int cal_perc; - // Used to show gps planner status bool gps_planner_active; @@ -130,6 +160,8 @@ typedef struct UIState { int font_courbd; int font_sans_regular; int font_sans_semibold; + int font_sans_bold; + int img_wheel; zsock_t *thermal_sock; void *thermal_sock_raw; @@ -146,6 +178,9 @@ typedef struct UIState { zsock_t *plus_sock; void *plus_sock_raw; + zsock_t *uilayout_sock; + void *uilayout_sock_raw; + int plus_state; // vision state @@ -185,6 +220,7 @@ typedef struct UIState { int status; bool is_metric; bool passive; + int alert_size; float light_sensor; } UIState; @@ -273,7 +309,7 @@ static const mat4 device_transform = {{ // frame from 4/3 to box size with a 2x zoon static const mat4 frame_transform = {{ - 2*(4./3.)/((float)box_width/box_height), 0.0, 0.0, 0.0, + 2*(4./3.)/((float)(box_w+sbr_w-(bdr_s*2))/viz_h), 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, @@ -299,6 +335,10 @@ static void ui_init(UIState *s) { assert(s->live100_sock); s->live100_sock_raw = zsock_resolve(s->live100_sock); + s->uilayout_sock = zsock_new_sub(">tcp://127.0.0.1:8060", ""); + assert(s->uilayout_sock); + s->uilayout_sock_raw = zsock_resolve(s->uilayout_sock); + s->livecalibration_sock = zsock_new_sub(">tcp://127.0.0.1:8019", ""); assert(s->livecalibration_sock); s->livecalibration_sock_raw = zsock_resolve(s->livecalibration_sock); @@ -334,6 +374,11 @@ static void ui_init(UIState *s) { assert(s->font_sans_regular >= 0); s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf"); assert(s->font_sans_semibold >= 0); + s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/OpenSans-Bold.ttf"); + assert(s->font_sans_bold >= 0); + + assert(s->img_wheel >= 0); + s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); @@ -627,7 +672,7 @@ static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, s } static void draw_path(UIState *s, const float *points, float off, - NVGcolor color) { + NVGcolor color, int width) { const UIScene *scene = &s->scene; nvgSave(s->vg); @@ -643,7 +688,7 @@ static void draw_path(UIState *s, const float *points, float off, nvgBeginPath(s->vg); nvgStrokeColor(s->vg, color); - nvgStrokeWidth(s->vg, 5); + nvgStrokeWidth(s->vg, width); bool started = false; for (int i=0; i<50; i++) { @@ -673,22 +718,21 @@ static void draw_path(UIState *s, const float *points, float off, } static void draw_model_path(UIState *s, const PathData path, NVGcolor color) { + draw_path(s, path.points, 0.0, color, 4*path.prob); float var = min(path.std, 0.7); - draw_path(s, path.points, 0.0, color); color.a /= 4; - draw_path(s, path.points, -var, color); - draw_path(s, path.points, var, color); + draw_path(s, path.points, -var, color, 2); + draw_path(s, path.points, var, color, 2); } static void draw_steering(UIState *s, float curvature) { - float points[50]; for (int i = 0; i < 50; i++) { float y_actual = i * tan(asin(clamp(i * curvature, -0.999, 0.999)) / 2.); points[i] = y_actual; } - draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128)); + draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5); } static void draw_frame(UIState *s) { @@ -709,10 +753,10 @@ static void draw_frame(UIState *s) { } else { out_mat = matmul(device_transform, frame_transform); - x1 = 0.0; - x2 = 1.0; - y1 = 0.0; - y2 = 1.0; + x1 = 1.0; + x2 = 0.0; + y1 = 1.0; + y2 = 0.0; } const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; @@ -779,27 +823,101 @@ static void ui_draw_rounded_rect( nvgStroke(c); } -// Draw all world space objects. -static void ui_draw_world(UIState *s) { +static void fill_lane(UIState *s, const float *l_points, const float *r_points, float off, + NVGcolor color) { const UIScene *scene = &s->scene; - if (!scene->world_objects_visible) { - return; + + nvgSave(s->vg); + + // path coords are worked out in rgb-box space + nvgTranslate(s->vg, 240.0f, 0.0); + + // zoom in 2x + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + + nvgBeginPath(s->vg); + bool started = false; + + int gradient_start_x = 0; + int gradient_start_y = 0; + int gradient_end_x = 0; + int gradient_end_y = 0; + + // left side of lane + for (int i=0; i<50; i++) { + float px = (float)i; + float py = l_points[i] + off; + + vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + if (x < 0 || y < 0.) { + continue; + } + + if (!started) { + nvgMoveTo(s->vg, x, y); + gradient_start_x = x; + gradient_start_y = y; + started = true; + } else { + nvgLineTo(s->vg, x, y); + } } - //draw_steering(s, scene->curvature); + // right side of lane + for (int i=50; i>0; i--) { + float px = (float)i; + float py = r_points[i] + off; + vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + if (x < 0 || y < 0.) { + continue; + } + + if (!started) { + nvgMoveTo(s->vg, x, y); + started = true; + } else { + nvgLineTo(s->vg, x, y); + gradient_end_y = y; + gradient_end_x = x; + } + } + + NVGpaint bg = nvgLinearGradient(s->vg, gradient_start_x, gradient_start_y, gradient_end_x, gradient_end_y, + nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.left_lane.prob), nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.right_lane.prob)); + + nvgFillPaint(s->vg, bg); + nvgFill(s->vg); + + nvgRestore(s->vg); +} + +static void ui_draw_vision_lanes(UIState *s) { + const UIScene *scene = &s->scene; if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { - int left_lane_color = (int)(255 * scene->model.left_lane.prob); - int right_lane_color = (int)(255 * scene->model.right_lane.prob); + // draw left lane edge draw_model_path( s, scene->model.left_lane, - nvgRGBA(left_lane_color, left_lane_color, left_lane_color, 128)); + nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); + + // draw right lane edge draw_model_path( s, scene->model.right_lane, - nvgRGBA(right_lane_color, right_lane_color, right_lane_color, 128)); + nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); - // draw paths - draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255)); + // draw projected path line + draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255), 5); // draw MPC only if engaged if (scene->engaged) { @@ -808,241 +926,436 @@ static void ui_draw_world(UIState *s) { } } +static void ui_draw_vision_topbar(UIState *s) { + const UIScene *scene = &s->scene; + + const int bar_x = box_x; + const int bar_y = box_y; + const int bar_width = box_w; + const int bar_height = 250 - box_y; + + assert(s->status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[s->status]; + + nvgBeginPath(s->vg); + nvgRect(s->vg, bar_x, bar_y, bar_width, bar_height); + nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); + nvgFill(s->vg); + + const int message_y = box_y; + const int message_height = bar_height; + const int message_width = 800; + const int message_x = box_x + box_w / 2 - message_width / 2; + + // message background + nvgBeginPath(s->vg); + NVGpaint bg = nvgLinearGradient(s->vg, message_x, message_y, message_x, message_y+message_height, + nvgRGBAf(0.0, 0.0, 0.0, 0.0), nvgRGBAf(0.0, 0.0, 0.0, 0.1)); + nvgFillPaint(s->vg, bg); + nvgRect(s->vg, message_x, message_y, message_width, message_height); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + + if (s->passive) { + if (s->scene.started_ts > 0) { + // draw drive time when passive + + uint64_t dt = nanos_since_boot() - s->scene.started_ts; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + char time_str[64]; + if (dt > 60*60*1000000000ULL) { + // hours + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d:%02d", + (int)(dt/(60*60*1000000000ULL)), + (int)((dt%(60*60*1000000000ULL))/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } else { + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d", + (int)(dt/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, time_str, NULL); + } + } else { + // status text + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 48*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + if (s->scene.alert_size == cereal_Live100Data_AlertSize_small) { + nvgFontSize(s->vg, 40*2.5); + nvgText(s->vg, message_x+message_width/2, 115, s->scene.alert_text1, NULL); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, message_x+message_width/2, 185, s->scene.alert_text2, NULL); + } else if (s->status == STATUS_DISENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "DISENGAGED", NULL); + } else if (s->status == STATUS_ENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "ENGAGED", NULL); + } + } + + // set speed + const int left_x = bar_x; + const int left_y = bar_y; + const int left_width = (bar_width - message_width) / 2; + const int left_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->v_cruise != 255 && scene->v_cruise != 0) { + char speed_str[16]; + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%3d kph", + (int)(scene->v_cruise + 0.5)); + } else { + /* Convert KPH to MPH. Using an approximated mph to kph + conversion factor of 1.60642 because this is what the Honda + hud seems to be using */ + snprintf(speed_str, sizeof(speed_str), "%3d mph", + (int)(scene->v_cruise * 0.6225 + 0.5)); + } + nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL); + } else { + nvgText(s->vg, left_x+left_width/2, 115, "N/A", NULL); + } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, left_x+left_width/2, 185, "SET SPEED", NULL); + + // lead car + const int right_y = bar_y; + const int right_width = (bar_width - message_width) / 2; + const int right_x = bar_x+bar_width-right_width; + const int right_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->lead_status) { + char radar_str[16]; + // lead car is always in meters + if (s->is_metric || true) { + snprintf(radar_str, sizeof(radar_str), "%d m", (int)scene->lead_d_rel); + } else { + snprintf(radar_str, sizeof(radar_str), "%d ft", (int)(scene->lead_d_rel * 3.28084)); + } + nvgText(s->vg, right_x+right_width/2, 115, radar_str, NULL); + } else { + nvgText(s->vg, right_x+right_width/2, 115, "N/A", NULL); + } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL); +} + +static void ui_draw_calibration_status(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + + int viz_calib_w = 1120; + int viz_calib_x = (ui_viz_rx + ((ui_viz_rw/2) - (viz_calib_w/2))); + int viz_calib_y = 760; + int viz_calib_h = 250; + int viz_calibtext_x = (ui_viz_rx + (ui_viz_rw/2)); + + nvgBeginPath(s->vg); + nvgStrokeWidth(s->vg, 10); + nvgRoundedRect(s->vg, viz_calib_x, viz_calib_y, viz_calib_w, viz_calib_h, 20); + nvgStroke(s->vg); + nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); + nvgFill(s->vg); + + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-semibold"); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); + char calib_status_str[64]; + snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc); + + nvgText(s->vg, viz_calibtext_x, viz_calib_y+100, calib_status_str, NULL); + if (s->is_metric) { + nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 72 km/h", NULL); + } else { + nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 45 mph", NULL); + } +} + +static void ui_draw_gpsplanner_status(UIState *s) { + const UIScene *scene = &s->scene; + + int rec_width = 1120; + int x_pos = 500; + nvgBeginPath(s->vg); + nvgStrokeWidth(s->vg, 14); + nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20); + nvgStroke(s->vg); + nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); + nvgFill(s->vg); + + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-semibold"); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); + nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL); +} + + +// Draw all world space objects. +static void ui_draw_world(UIState *s) { + const UIScene *scene = &s->scene; + if (!scene->world_objects_visible) { + return; + } + + //draw_steering(s, scene->curvature); + ui_draw_vision_lanes(s); + +} + +static void ui_draw_vision_maxspeed(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + float maxspeed = s->scene.v_cruise; + + const int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2)); + const int viz_maxspeed_y = (viz_y + (bdr_s*1.5)); + const int viz_maxspeed_w = 180; + const int viz_maxspeed_h = 202; + char maxspeed_str[32]; + bool is_cruise_set = (maxspeed != 0 && maxspeed != 255); + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 148, "MAX", NULL); + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 52*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + if (is_cruise_set) { + if (s->is_metric) { + snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed + 0.5)); + } else { + snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed * 0.6225 + 0.5)); + } + nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, maxspeed_str, NULL); + } else { + nvgFontSize(s->vg, 42*2.5); + nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, "N/A", NULL); + } +} + +static void ui_draw_vision_speed(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + float speed = s->scene.v_ego; + + const int viz_speed_w = 280; + const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2)); + char speed_str[32]; + + nvgBeginPath(s->vg); + nvgRect(s->vg, viz_speed_x, viz_y, viz_speed_w, viz_header_h); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); + } else { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2374144 + 0.5)); + } + nvgFontFace(s->vg, "sans-bold"); + nvgFontSize(s->vg, 96*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 36*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL); +} + +static void ui_draw_vision_wheel(UIState *s) { + const UIScene *scene = &s->scene; + const int ui_viz_rx = scene->ui_viz_rx; + const int ui_viz_rw = scene->ui_viz_rw; + const int viz_event_w = 220; + const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2))); + const int viz_event_y = (viz_y + (bdr_s*1.5)); + const int viz_event_h = (viz_header_h - (bdr_s*1.5)); + // draw steering wheel + const int viz_wheel_size = 96; + const int viz_wheel_x = viz_event_x + (viz_event_w-viz_wheel_size); + const int viz_wheel_y = viz_event_y + (viz_wheel_size/2); + const int img_wheel_size = viz_wheel_size*1.5; + const int img_wheel_x = viz_wheel_x-(img_wheel_size/2); + const int img_wheel_y = viz_wheel_y-25; + float img_wheel_alpha = 0.5f; + bool is_engaged = (s->status == STATUS_ENGAGED); + bool is_warning = (s->status == STATUS_WARNING); + if (is_engaged || is_warning) { + nvgBeginPath(s->vg); + nvgCircle(s->vg, viz_wheel_x, (viz_wheel_y + (bdr_s*1.5)), viz_wheel_size); + if (is_engaged) { + nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); + } else if (is_warning) { + nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); + } + nvgFill(s->vg); + img_wheel_alpha = 1.0f; + } + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y, + img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); + nvgRect(s->vg, img_wheel_x, img_wheel_y, img_wheel_size, img_wheel_size); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_vision_header(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + + nvgBeginPath(s->vg); + NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx, + (viz_y+(viz_header_h-(viz_header_h/2.5))), + ui_viz_rx, viz_y+viz_header_h, + nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0)); + nvgFillPaint(s->vg, gradient); + nvgRect(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_header_h); + nvgFill(s->vg); + + ui_draw_vision_maxspeed(s); + ui_draw_vision_speed(s); + ui_draw_vision_wheel(s); +} + +static void ui_draw_vision_alert(UIState *s) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + bool mapEnabled = s->scene.uilayout_mapenabled; + bool longAlert1 = strlen(scene->alert_text1) > 15; + + const uint8_t *color = alert_colors[s->status]; + const int alr_s = alert_sizes[s->alert_size]; + const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)-bdr_s; + const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)+(bdr_s*2); + const int alr_h = alr_s+(s->alert_size==ALERTSIZE_NONE?0:bdr_s); + const int alr_y = vwp_h-alr_h; + + nvgBeginPath(s->vg); + nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); + nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],color[3])); + nvgFill(s->vg); + + nvgBeginPath(s->vg); + NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, + nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); + nvgFillPaint(s->vg, gradient); + nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (s->alert_size == ALERTSIZE_SMALL) { + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, scene->alert_text1, NULL); + } else if (s->alert_size == ALERTSIZE_MID) { + nvgFontFace(s->vg, "sans-bold"); + nvgFontSize(s->vg, 48*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, scene->alert_text1, NULL); + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 36*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, scene->alert_text2, NULL); + } else if (s->alert_size == ALERTSIZE_FULL) { + nvgFontSize(s->vg, (longAlert1?72:96)*2.5); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, scene->alert_text1, NULL); + nvgFontSize(s->vg, 48*2.5); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); + nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, scene->alert_text2, NULL); + } +} + static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; glClearColor(0.0, 0.0, 0.0, 0.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - // hack for eon ui glEnable(GL_SCISSOR_TEST); - glScissor(box_x, s->fb_h-(box_y+box_height), box_width, box_height); - glViewport(box_x, s->fb_h-(box_y+box_height), box_width, box_height); + const int viewport_rw = (box_w+sbr_w-(bdr_s*2)); + glViewport(ui_viz_rx + scene->ui_frame_offset, s->fb_h-(viz_y+viz_h), viewport_rw - scene->ui_frame_offset, viz_h); + glScissor(ui_viz_rx, s->fb_h-(viz_y+viz_h), ui_viz_rw, viz_h); draw_frame(s); glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_SCISSOR_TEST); - // nvg drawings glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - // glEnable(GL_CULL_FACE); - glClear(GL_STENCIL_BUFFER_BIT); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - nvgSave(s->vg); - // hack for eon ui - const int inner_height = box_width*9/16; - nvgScissor(s->vg, box_x, box_y, box_width, box_height); - nvgTranslate(s->vg, box_x, box_y + (box_height-inner_height)/2.0); - nvgScale(s->vg, (float)box_width / s->fb_w, (float)inner_height / s->fb_h); + // hack for eon + const int inner_height = viewport_rw*9/16; + nvgScissor(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_h); + nvgTranslate(s->vg, ui_viz_rx + scene->ui_world_offset, viz_y + (viz_h-inner_height)/2.0); + nvgScale(s->vg, (float)viewport_rw / s->fb_w, (float)inner_height / s->fb_h); if (!scene->frontview) { - // ui_draw_transformed_box(s, 0xFF00FF00); ui_draw_world(s); if (scene->lead_status) { draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30, nvgRGBA(255, 0, 0, 128)); } - - const float label_size = 65.0f; - - nvgFontFace(s->vg, "courbd"); - - if (scene->awareness_status > 0) { - nvgBeginPath(s->vg); - int bar_height = scene->awareness_status * 700; - nvgRect(s->vg, 100, 300 + (700 - bar_height), 50, bar_height); - nvgFillColor(s->vg, nvgRGBA(255 * (1 - scene->awareness_status), - 255 * scene->awareness_status, 0, 128)); - nvgFill(s->vg); - } - - // Draw calibration progress (if needed) - if (scene->cal_status == CALIBRATION_UNCALIBRATED) { - int rec_width = 1120; - int x_pos = 500; - nvgBeginPath(s->vg); - nvgStrokeWidth(s->vg, 14); - nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20); - nvgStroke(s->vg); - nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); - nvgFill(s->vg); - - nvgFontSize(s->vg, 40*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-semibold"); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); - char calib_status_str[64]; - snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc); - - nvgText(s->vg, x_pos, 1010, calib_status_str, NULL); - if (s->is_metric) { - nvgText(s->vg, x_pos + 120, 1110, "Drive above 72 km/h", NULL); - } else { - nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL); - } - } else if (scene->gps_planner_active) { - int rec_width = 1120; - int x_pos = 500; - nvgBeginPath(s->vg); - nvgStrokeWidth(s->vg, 14); - nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20); - nvgStroke(s->vg); - nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); - nvgFill(s->vg); - - nvgFontSize(s->vg, 40*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-semibold"); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); - nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL); - } } nvgRestore(s->vg); + // draw vision elements + ui_draw_vision_header(s); + ui_draw_vision_alert(s); - if (!ui_alert_active(s) && !scene->frontview) { - // draw top bar - - const int bar_x = box_x; - const int bar_y = box_y; - const int bar_width = box_width; - const int bar_height = 250 - box_y; - - assert(s->status < ARRAYSIZE(bg_colors)); - const uint8_t *color = bg_colors[s->status]; - - nvgBeginPath(s->vg); - nvgRect(s->vg, bar_x, bar_y, bar_width, bar_height); - nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); - nvgFill(s->vg); - - const int message_y = box_y; - const int message_height = bar_height; - const int message_width = 800; - const int message_x = box_x + box_width / 2 - message_width / 2; - - // message background - nvgBeginPath(s->vg); - NVGpaint bg = nvgLinearGradient(s->vg, message_x, message_y, message_x, message_y+message_height, - nvgRGBAf(0.0, 0.0, 0.0, 0.0), nvgRGBAf(0.0, 0.0, 0.0, 0.1)); - nvgFillPaint(s->vg, bg); - nvgRect(s->vg, message_x, message_y, message_width, message_height); - nvgFill(s->vg); - - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - - if (s->passive) { - if (s->scene.started_ts > 0) { - // draw drive time when passive - - uint64_t dt = nanos_since_boot() - s->scene.started_ts; - - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 40*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - - char time_str[64]; - if (dt > 60*60*1000000000ULL) { - // hours - snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d:%02d", - (int)(dt/(60*60*1000000000ULL)), - (int)((dt%(60*60*1000000000ULL))/(60*1000000000ULL)), - (int)(dt%(60*1000000000ULL)/1000000000ULL)); - } else { - snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d", - (int)(dt/(60*1000000000ULL)), - (int)(dt%(60*1000000000ULL)/1000000000ULL)); - } - nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, time_str, NULL); - } - } else { - // status text - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 48*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - if (s->scene.alert_size == cereal_Live100Data_AlertSize_small) { - nvgFontSize(s->vg, 40*2.5); - nvgText(s->vg, message_x+message_width/2, 115, s->scene.alert_text1, NULL); - nvgFontSize(s->vg, 26*2.5); - nvgText(s->vg, message_x+message_width/2, 185, s->scene.alert_text2, NULL); - } else if (s->status == STATUS_DISENGAGED) { - nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "DISENGAGED", NULL); - } else if (s->status == STATUS_ENGAGED) { - nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "ENGAGED", NULL); - } - } - - // set speed - const int left_x = bar_x; - const int left_y = bar_y; - const int left_width = (bar_width - message_width) / 2; - const int left_height = bar_height; - - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 40*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - - if (scene->v_cruise != 255 && scene->v_cruise != 0) { - char speed_str[16]; - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d kph", - (int)(scene->v_cruise + 0.5)); - } else { - /* Convert KPH to MPH. Using an approximated mph to kph - conversion factor of 1.609 because this is what the Honda - hud seems to be using */ - snprintf(speed_str, sizeof(speed_str), "%3d mph", - (int)(scene->v_cruise * 0.621504 + 0.5)); - } - nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL); - } else { - nvgText(s->vg, left_x+left_width/2, 115, "N/A", NULL); - } - - nvgFontFace(s->vg, "sans-regular"); - nvgFontSize(s->vg, 26*2.5); - nvgText(s->vg, left_x+left_width/2, 185, "SET SPEED", NULL); - - // lead car - const int right_y = bar_y; - const int right_width = (bar_width - message_width) / 2; - const int right_x = bar_x+bar_width-right_width; - const int right_height = bar_height; - - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 40*2.5); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - - if (scene->lead_status) { - char radar_str[16]; - // lead car is always in meters - if (s->is_metric || true) { - snprintf(radar_str, sizeof(radar_str), "%d m", (int)scene->lead_d_rel); - } else { - snprintf(radar_str, sizeof(radar_str), "%d ft", (int)(scene->lead_d_rel * 3.28084)); - } - nvgText(s->vg, right_x+right_width/2, 115, radar_str, NULL); - } else { - nvgText(s->vg, right_x+right_width/2, 115, "N/A", NULL); - } - - nvgFontFace(s->vg, "sans-regular"); - nvgFontSize(s->vg, 26*2.5); - nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL); + // Draw calibration progress (if needed) + if (scene->cal_status == CALIBRATION_UNCALIBRATED) { + ui_draw_calibration_status(s); } nvgEndFrame(s->vg); - glDisable(GL_BLEND); - // glDisable(GL_CULL_FACE); } + static void ui_draw_alerts(UIState *s) { const UIScene *scene = &s->scene; @@ -1057,7 +1370,7 @@ static void ui_draw_alerts(UIState *s) { } nvgBeginPath(s->vg); - nvgRect(s->vg, box_x, box_y, box_width, box_height); + nvgRect(s->vg, box_x, box_y, box_w, box_h); nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); nvgFill(s->vg); @@ -1070,7 +1383,7 @@ static void ui_draw_alerts(UIState *s) { } nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, box_x + 50, box_y + 287, box_width - 50, alert_text1_upper, NULL); + nvgTextBox(s->vg, box_x + 50, box_y + 287, box_w - 50, alert_text1_upper, NULL); if (strlen(scene->alert_text2) > 0) { @@ -1079,7 +1392,7 @@ static void ui_draw_alerts(UIState *s) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgFontSize(s->vg, 44.0*2.5); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); - nvgTextBox(s->vg, box_x + 50, box_y + box_height - 250, box_width - 50, scene->alert_text2, NULL); + nvgTextBox(s->vg, box_x + 50, box_y + box_h - 250, box_w - 50, scene->alert_text2, NULL); } } @@ -1138,10 +1451,10 @@ static void ui_draw(UIState *s) { nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - if (s->vision_connected) { + if (s->vision_connected && !s->scene.uilayout_sidebarcollapsed) { ui_draw_aside(s); } - ui_draw_alerts(s); + // ui_draw_alerts(s); nvgEndFrame(s->vg); glDisable(GL_BLEND); @@ -1235,12 +1548,18 @@ static void ui_update(UIState *s) { assert(glGetError() == GL_NO_ERROR); + // Default UI Measurements (Assumes sidebar visible) + s->scene.ui_viz_rx = box_x; + s->scene.ui_viz_rw = box_w; + s->scene.ui_frame_offset = -(sbr_w - 4*bdr_s); + s->scene.ui_world_offset = -(sbr_w - 6*bdr_s); + s->vision_connect_firstrun = false; } // poll for events while (true) { - zmq_pollitem_t polls[8] = {{0}}; + zmq_pollitem_t polls[9] = {{0}}; polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -1253,15 +1572,16 @@ static void ui_update(UIState *s) { polls[4].events = ZMQ_POLLIN; polls[5].socket = s->thermal_sock_raw; polls[5].events = ZMQ_POLLIN; - - polls[6].socket = s->plus_sock_raw; + polls[6].socket = s->uilayout_sock_raw; polls[6].events = ZMQ_POLLIN; + polls[7].socket = s->plus_sock_raw; + polls[7].events = ZMQ_POLLIN; - int num_polls = 7; + int num_polls = 8; if (s->vision_connected) { assert(s->ipc_fd >= 0); - polls[7].fd = s->ipc_fd; - polls[7].events = ZMQ_POLLIN; + polls[8].fd = s->ipc_fd; + polls[8].events = ZMQ_POLLIN; num_polls++; } @@ -1275,12 +1595,12 @@ static void ui_update(UIState *s) { } if (polls[0].revents || polls[1].revents || polls[2].revents || - polls[3].revents || polls[4].revents) { + polls[3].revents || polls[4].revents || polls[6].revents || polls[7].revents) { // awake on any (old) activity set_awake(s, true); } - if (s->vision_connected && polls[7].revents) { + if (s->vision_connected && polls[8].revents) { // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1329,7 +1649,7 @@ static void ui_update(UIState *s) { } else { assert(false); } - } else if (polls[6].revents) { + } else if (polls[7].revents) { // plus socket zmq_msg_t msg; @@ -1347,7 +1667,7 @@ static void ui_update(UIState *s) { } else { // zmq messages void* which = NULL; - for (int i=0; i<6; i++) { + for (int i=0; iscene.alert_ts = eventd.logMonoTime; s->scene.alert_size = datad.alertSize; + if (datad.alertSize == cereal_Live100Data_AlertSize_none) { + s->alert_size = ALERTSIZE_NONE; + } else if (datad.alertSize == cereal_Live100Data_AlertSize_small) { + s->alert_size = ALERTSIZE_SMALL; + } else if (datad.alertSize == cereal_Live100Data_AlertSize_mid) { + s->alert_size = ALERTSIZE_MID; + } else if (datad.alertSize == cereal_Live100Data_AlertSize_full) { + s->alert_size = ALERTSIZE_FULL; + } if (datad.alertStatus == cereal_Live100Data_AlertStatus_userPrompt) { update_status(s, STATUS_WARNING); @@ -1474,12 +1803,26 @@ static void ui_update(UIState *s) { } s->scene.started_ts = datad.startedTs; + } else if (eventd.which == cereal_Event_uiLayoutState) { + struct cereal_UiLayoutState datad; + cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); + s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; + s->scene.uilayout_mapenabled = datad.mapEnabled; + + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + bool mapEnabled = s->scene.uilayout_mapenabled; + if (mapEnabled) { + s->scene.ui_viz_rx = hasSidebar ? viz_x : (viz_x-(bdr_s*2)); + s->scene.ui_viz_rw = hasSidebar ? viz_w : (viz_w+(bdr_s*2)); + } else { + s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); + s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); + s->scene.ui_frame_offset = hasSidebar ? -(sbr_w - 4*bdr_s) : -(bdr_s*2); + s->scene.ui_world_offset = hasSidebar ? -(sbr_w - 6*bdr_s) : -(bdr_s*2); + } } - capn_free(&ctx); - zmq_msg_close(&msg); - } } @@ -1676,6 +2019,7 @@ int main() { if (EON) { // light sensor is only exposed on EONs + float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B; if (clipped_light_sensor > 255) clipped_light_sensor = 255; smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 516fa31b..67a59299 100644 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -8,6 +8,7 @@ import subprocess from common.basedir import BASEDIR from selfdrive.swaglog import cloudlog +from selfdrive.version import dirty def main(gctx=None): while True: @@ -17,7 +18,13 @@ def main(gctx=None): time.sleep(60) continue - r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"]) + # If there are modifications we want to full history + # otherwise only store head to save space + if dirty: + r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--unshallow"]) + else: + r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"]) + cloudlog.info("git fetch: %r", r) if r: time.sleep(60) diff --git a/selfdrive/version.py b/selfdrive/version.py index b5d667f3..9c47ecfc 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -1,3 +1,17 @@ import os +import subprocess with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: version = _versionf.read().split('"')[1] + +try: + if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]): + upstream = "origin/master" + else: + if 'chffrplus' in version: + upstream = "origin/release" + else: + upstream = "origin/release2" + + dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0 +except subprocess.CalledProcessError: + dirty = True diff --git a/selfdrive/visiond/README b/selfdrive/visiond/README index ae92bd4b..d2375627 100644 --- a/selfdrive/visiond/README +++ b/selfdrive/visiond/README @@ -1,3 +1,3 @@ -visiond runs the openpilot vision pipeline. Everything running between the camera hardware and model outputs lives here. +visiond runs the openpilot/chffrplus vision pipeline. Everything running between the camera hardware and model outputs lives here. Contact us if you'd like features added or support for your platform. diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 2a8b8e1e..b9e752d1 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ