openpilot v0.4.3 release

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)
+

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:
 

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.
+

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

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',

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 {

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;
   }
 }

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: {
+  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,
+    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,
+    0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5,
-  },
+  }],
-  HONDA.ACURA_RDX: {
+  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: {
+  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,
+    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,
+    0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
-  },
+  }],
-  HONDA.CRV: {
+  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: {
+  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
+    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

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:
+    for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]):
-      ms = self.cp[n]
       if n in self.cp_ignored:
-        print "%30s: %7.2f   perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), "  IGNORED"
+        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)
 

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)

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

launch_openpilot.sh

@@ -1,39 +1,5 @@
 #!/usr/bin/bash
 
-function launch {
+export PASSIVE="0"
-  # apply update
+exec ./launch_chffrplus.sh
-  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
-
-  # 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

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

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<cereal::Event>();
+  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
+      pigeon_publish_raw(publisher, dat, alen);
-      capnp::MallocMessageBuilder msg;
-      cereal::Event::Builder event = msg.initRoot<cereal::Event>();
-      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);
     }
 
     // 10ms

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"

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);
 

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):

selfdrive/can/parser.cc

@@ -142,18 +142,23 @@ class CANParser {
   CANParser(int abus, const std::string& dbc_name,
             const std::vector<MessageParseOptions> &options,
             const std::vector<SignalParseOptions> &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<MessageParseOptions>(message_options, message_options+num_message_options)
                                   : std::vector<MessageParseOptions>{}),
                                  (signal_options ? std::vector<SignalParseOptions>(signal_options, signal_options+num_signal_options)
-                                  : std::vector<SignalParseOptions>{}), sendcan);
+                                  : std::vector<SignalParseOptions>{}), sendcan, std::string(tcp_addr));
   return (void*)ret;
 }
 

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*")
 

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",
+                     ["pcm_accel", "v_cruise", "mini_car", "car", "X4",
-                      "lanes", "beep", "X8", "chime", "acc_alert"])
+                      "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,
+    hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
-                  0xc1, 0x41, hud_lanes + steer_required,
+                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
-                  int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert)
 
     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,
+        hondacan.create_brake_command(self.packer, apply_brake, pcm_override,
-                                      pcm_cancel_cmd, hud.chime, idx))
+                                      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(self.packer, apply_gas, idx))
-        can_sends.append(hondacan.create_gas_command(gas_amount, 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:

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]])

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 = {
-def create_gas_command(gas_amount, idx):
+    "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(packer, gas_amount, idx):
   """Creates a CAN message for the Honda DBC GAS_COMMAND."""
-  msg = struct.pack("!H", gas_amount)
+  enable = gas_amount > 0.001
-  return make_can_msg(0x200, msg, idx, 0)
+
+  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(apply_steer, car_fingerprint, idx):
+def create_steering_control(packer, apply_steer, car_fingerprint, idx):
   """Creates a CAN message for the Honda DBC STEERING_CONTROL."""
-  commands = []
+  values = {
-  if car_fingerprint in (CAR.CRV, CAR.ACURA_RDX):
+    "STEER_TORQUE": apply_steer,
-    msg_0x194 = struct.pack("!h", apply_steer << 4) + ("\x80" if apply_steer != 0 else "\x00")
+    "STEER_TORQUE_REQUEST": apply_steer != 0,
-    commands.append(make_can_msg(0x194, msg_0x194, idx, 0))
+  }
-  else:
+  return packer.make_can_msg("STEERING_CONTROL", 0, values, idx)
-    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
 
 
-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
+  # TODO: Why is X4 always 0xc1? Not implemented yet in canpacker
-  msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
+  acc_hud_values = {
-                          hud.v_cruise, hud.X2, hud.car, hud.X4)
+    'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
-  commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
+    'PCM_GAS': hud.pcm_accel,
-
+    'CRUISE_SPEED': hud.v_cruise,
-  msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
+    'ENABLE_MINI_CAR': hud.mini_car,
-  commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
+    '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(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx))
-    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))
+    radar_hud_values = {
-    commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
+      '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

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]))
 

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]

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_MAX = 1.5  # 1.5 m/s2
-ACCEL_MIN = -3000 # 3   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_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_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_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))
 
-    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:
+    # 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
+
     # 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_ui_command(self.packer, steer, sound1, sound2))
-      can_sends.append(create_fcw_command(fcw))
+      can_sends.append(create_fcw_command(self.packer, fcw))
 
     #*** static msgs ***
 

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']
 

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

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
+    direction = 3
-    steer = 0xfff + steer + 1
   elif steer > 0:
-    move = 0x20
+    direction = 1
   else:
-    move = 0x40
+    direction = 2
-
-  mode = 0x30 if steer else 0x10
 
-  steer_h = (steer & 0xF00) >> 8
+  mode = 3 if enabled else 1
-  steer_l = steer & 0xff
 
-  msg = struct.pack("!BBBBBBB", mode | steer_h, steer_l, 0x10, 0x00, move, 0x40, 0x00)
+  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)
 
-  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)
+  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)
 
-  return make_can_msg(0x2e4, msg, 0, True)
 
-
+def create_accel_command(packer, accel, pcm_cancel, standstill_req):
-def create_accel_command(accel, pcm_cancel, standstill_req):
   # TODO: find the exact canceling bit
-  state = 0x40 if standstill_req else 0xC0
+  values = {
-  state += pcm_cancel # this allows automatic restart from hold without driver cmd
+    "ACCEL_CMD": accel,
-
+    "SET_ME_X63": 0x63,
-  msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
+    "SET_ME_1": 1,
-
+    "RELEASE_STANDSTILL": not standstill_req,
-  return make_can_msg(0x343, msg, 0, True)
+    "CANCEL_REQ": pcm_cancel,
-
+  }
-def create_fcw_command(fcw):
+  return packer.make_can_msg("ACC_CONTROL", 0, values)
-
+
-  msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00)
+
-
+def create_fcw_command(packer, fcw):
-  return make_can_msg(0x411, msg, 0, False)
+  values = {
-
+    "FCW": fcw,
-
+    "SET_ME_X20": 0x20,
-def create_ui_command(steer, sound1, sound2):
+    "SET_ME_X10": 0x10,
-
+    "SET_ME_X80": 0x80,
-  msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00,
+  }
-                                 sound1, 0x2C, 0x38, 0x02)
+  return packer.make_can_msg("ACC_HUD", 0, values)
-  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)

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.4.2-openpilot"
+#define COMMA_VERSION "0.4.3-release"

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 {

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,
+  actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
-                               CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
+                                                     CS.steeringPressed, plan.dPoly, angle_offset, 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")
 

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!", 
+        "Brake!",
-        "Risk of Collision", 
+        "Risk of collision detected",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
 
     "steerSaturated": Alert(
-        "Take Control", 
+        "TAKE CONTROL",
-        "Turn Exceeds Limit", 
+        "Turn exceeds steering limit",
-        AlertStatus.userPrompt, AlertSize.full,
+        AlertStatus.userPrompt, AlertSize.mid,
         Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
 
     "steerTempUnavailable": Alert(
-        "Take Control", 
+        "TAKE CONTROL",
-        "Steer Temporarily Unavailable", 
+        "Steering temporarily unavailable",
-        AlertStatus.userPrompt, AlertSize.full,
+        AlertStatus.userPrompt, AlertSize.mid,
         Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
 
     "preDriverDistracted": Alert(
-        "Take Control", 
+        "TAKE CONTROL",
-        "User Distracted", 
+        "User appears distracted",
-        AlertStatus.userPrompt, AlertSize.full,
+        AlertStatus.userPrompt, AlertSize.mid,
         Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
 
     "driverDistracted": Alert(
-        "Take Control to Regain Speed", 
+        "TAKE CONTROL TO REGAIN SPEED",
-        "User Distracted", 
+        "User appears distracted",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
 
     "startup": Alert(
-        "Always Keep Hands on Wheel", 
+        "Always keep hands on wheel",
-        "Be Ready to Take Over Any Time", 
+        "Be ready to take over at any time",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOWEST, None, None, 0., 0., 15.),
 
     "ethicalDilemma": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Ethical Dilemma Detected", 
+        "Ethical Dilemma Detected",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "steerTempUnavailableNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Steer Temporary Unavailable", 
+        "Steering temporarily unavailable",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "manualRestart": Alert(
-        "Take Control",
+        "TAKE CONTROL",
-        "Resume Driving Manually",
+        "Resume driving manually",
         AlertStatus.userPrompt, AlertSize.full,
         Priority.LOW, None, None, 0., 0., .2),
 
     # Non-entry only alerts
     "wrongCarModeNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Main Switch Off", 
+        "Main Switch Off",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "dataNeededNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Data needed for calibration. Upload drive, try again", 
+        "Data needed for calibration. Upload drive, try again",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "outOfSpaceNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Out of Space", 
+        "Out of storage space",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "pedalPressedNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Pedal Pressed", 
+        "Pedal pressed during attempt",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
 
     "speedTooLowNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Speed Too Low", 
+        "Speed too low",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "brakeHoldNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Brake Hold Active", 
+        "Brake hold active",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "parkBrakeNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Park Brake Engaged", 
+        "Park brake engaged",
-        AlertStatus.normal, AlertSize.full,
+        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", 
+        "TAKE CONTROL IMMEDIATELY",
-        "System Overheated", 
+        "System Overheated",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "wrongGear": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Gear not D", 
+        "Gear not D",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "calibrationInvalid": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Calibration Invalid: Reposition EON and Recalibrate", 
+        "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", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Door Open", 
+        "Door Open",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "seatbeltNotLatched": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Seatbelt Unlatched", 
+        "Seatbelt Unlatched",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "espDisabled": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "ESP Off", 
+        "ESP Off",
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     # Cancellation alerts causing immediate disabling
     "radarCommIssue": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Radar Error: Restart the Car", 
+        "Radar Error: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "radarFault": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Radar Error: Restart the Car", 
+        "Radar Error: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "modelCommIssue": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Model Error: Restart the Car", 
+        "Model Error: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "controlsFailed": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Controls Failed", 
+        "Controls Failed",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "controlsMismatch": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Controls Mismatch", 
+        "Controls Mismatch",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "commIssue": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "CAN Error: Restart the Car", 
+        "CAN Error: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "steerUnavailable": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Steer Fault: Restart the Car", 
+        "Steer Fault: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "brakeUnavailable": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Brake Fault: Restart the Car", 
+        "Brake Fault: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "gasUnavailable": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Gas Fault: Restart the Car", 
+        "Gas Fault: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "reverseGear": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Reverse Gear", 
+        "Reverse Gear",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "cruiseDisabled": Alert(
-        "Take Control Immediately", 
+        "TAKE CONTROL IMMEDIATELY",
-        "Cruise Is Off", 
+        "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", 
+        "No close lead car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
 
     "speedTooLow": Alert(
         "Comma Canceled",
-        "Speed Too Low", 
+        "Speed too low",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
 
     # Cancellation alerts causing non-entry
     "overheatNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "System Overheated", 
+        "System overheated",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "wrongGearNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Gear not D", 
+        "Gear not D",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "calibrationInvalidNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Calibration Invalid: Reposition EON and Recalibrate", 
+        "Calibration Invalid: Reposition EON and Recalibrate",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "calibrationInProgressNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Calibration in Progress", 
+        "Calibration in Progress",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "doorOpenNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Door Open", 
+        "Door open",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "seatbeltNotLatchedNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Seatbelt Unlatched", 
+        "Seatbelt unlatched",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
- 
+
     "espDisabledNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "ESP Off", 
+        "ESP Off",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "radarCommIssueNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Radar Error: Restart the Car", 
+        "Radar Error: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "radarFaultNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Radar Error: Restart the Car", 
+        "Radar Error: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "modelCommIssueNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Model Error: Restart the Car", 
+        "Model Error: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "controlsFailedNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Controls Failed", 
+        "Controls Failed",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "commIssueNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "CAN Error: Restart the Car", 
+        "CAN Error: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "steerUnavailableNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Steer Fault: Restart the Car", 
+        "Steer Fault: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "brakeUnavailableNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Brake Fault: Restart the Car", 
+        "Brake Fault: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "gasUnavailableNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Gas Error: Restart the Car", 
+        "Gas Error: Restart the Car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "reverseGearNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Reverse Gear", 
+        "Reverse Gear",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "cruiseDisabledNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "Cruise is Off", 
+        "Cruise is off",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "noTargetNoEntry": Alert(
-        "Comma Unavailable", 
+        "Comma Unavailable",
-        "No Close Lead",
+        "No close lead car",
-        AlertStatus.normal, AlertSize.full,
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     # permanent alerts to display on small UI upper box
     "steerUnavailablePermanent": Alert(
-        "STEER FAULT", 
+        "STEER FAULT: Restart the car to engage",
-        "RESTART THE CAR",
+        "",
         AlertStatus.normal, AlertSize.small,
         Priority.LOWEST, None, None, 0., 0., .2),
 
     "brakeUnavailablePermanent": Alert(
-        "BRAKE FAULT", 
+        "BRAKE FAULT: Restart the car to engage",
-        "RESTART THE CAR",
+        "",
         AlertStatus.normal, AlertSize.small,
         Priority.LOWEST, None, None, 0., 0., .2),
 
     "lowSpeedLockoutPermanent": Alert(
-        "CRUISE FAULT", 
+        "CRUISE FAULT: Restart the car to engage",
-        "RESTART THE CAR",
+        "",
         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

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'

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)

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;
 

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);

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 + 0] = pathCost * f;
-    acadoVariables.W[25 * i + 6] = 1.0 * f;
+    acadoVariables.W[25 * i + 6] = laneCost * f;
-    acadoVariables.W[25 * i + 12] = 1.0 * f;
+    acadoVariables.W[25 * i + 12] = laneCost * f;
-    acadoVariables.W[25 * i + 18] = 1.0 * f;
+    acadoVariables.W[25 * i + 18] = headingCost * f;
     acadoVariables.W[25 * i + 24] = steerRateCost * f;
   }
-  acadoVariables.WN[0] = 1.0;
+  acadoVariables.WN[0] = pathCost * STEP_MULTIPLIER;
-  acadoVariables.WN[5] = 1.0;
+  acadoVariables.WN[5] = laneCost * STEP_MULTIPLIER;
-  acadoVariables.WN[10] = 1.0;
+  acadoVariables.WN[10] = laneCost * STEP_MULTIPLIER;
-  acadoVariables.WN[15] = 1.0;
+  acadoVariables.WN[15] = headingCost * STEP_MULTIPLIER;
 }
 
 int run_mpc(state_t * x0, log_t * solution,

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -1,7 +1,6 @@
 #include <acado_code_generation.hpp>
 
 const int controlHorizon = 50;
-const double samplingTime = 0.2;
 
 using namespace std;
 

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);

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 + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) 
-    acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance
+    acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance
-    acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration
+    acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration
-    acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk
+    acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk
   }
-  acadoVariables.WN[0] = 5.0; // exponential cost for danger zone
+  acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone
-  acadoVariables.WN[4] = 0.1; // desired distance
+  acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance
-  acadoVariables.WN[8] = 10.0; // acceleration
+  acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration
 
 }
 

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 = [1.1, 1.1, .8, .5, .3]
-_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .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

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:

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)

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)

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:
+      msg = sock.recv()
-        sys.stdout.write(sock.recv())
+      evt = log.Event.from_bytes(msg)
-        sys.stdout.flush()
+      if sock in republish_socks:
-      elif args.raw:
+        republish_socks[sock].send(msg)
-        hexdump(sock.recv())
+      if args.map and evt.which() == 'liveLocation':
-      elif args.json:
+        print 'send loc'
-        print(json.loads(sock.recv()))
+        socketio.emit('location', { 
-      elif args.dump_json:
+          'lat': evt.liveLocation.lat,
-        print json.dumps(messaging.recv_one(sock).to_dict())
+          'lon': evt.liveLocation.lon,
-      else:
+          'alt': evt.liveLocation.alt,
-        print messaging.recv_one(sock)
+        })
+      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
 

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 <tnt@246tNt.com>
+    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
+
+

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()

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('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
+    msg._buf = header + msg._buf
+
+    for r in msg._recs:
+      f2 = []
+      for f in self.fields2:
+        f2.append(r[f])
+      msg._buf += struct.pack(self.format2, *tuple(f2))
+    msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
+
+  def format(self, msg):
+    '''return a formatted string for a message'''
+    if not msg._unpacked:
+      self.unpack(msg)
+    ret = self.name + ': '
+    for f in self.fields:
+      (fieldname, alen) = ArrayParse(f)
+      if not fieldname in msg._fields:
+        continue
+      v = msg._fields[fieldname]
+      if isinstance(v, list):
+        ret += '%s=[' % fieldname
+        for a in range(alen):
+          ret += '%s, ' % v[a]
+        ret = ret[:-2] + '], '
+      elif isinstance(v, str):
+        ret += '%s="%s", ' % (f, v.rstrip(' \0'))
+      else:
+        ret += '%s=%s, ' % (f, v)
+    for r in msg._recs:
+      ret += '[ '
+      for f in self.fields2:
+        v = r[f]
+        ret += '%s=%s, ' % (f, v)
+      ret = ret[:-2] + ' ], '
+    return ret[:-2]
+
+
+# list of supported message types.
+msg_types = {
+  (CLASS_ACK, MSG_ACK_ACK):
+  UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
+  (CLASS_ACK, MSG_ACK_NACK):
+  UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
+  (CLASS_CFG, MSG_CFG_USB):
+  UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
+    'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
+    'vendorString', 'productString', 'serialNumber'
+  ]),
+  (CLASS_CFG, MSG_CFG_PRT):
+  UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
+    'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
+    'reserved4', 'reserved5'
+  ]),
+  (CLASS_CFG, MSG_CFG_CFG):
+  UBloxDescriptor('CFG_CFG', '<III,B',
+                  ['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
+  (CLASS_CFG, MSG_CFG_RXM):
+  UBloxDescriptor('CFG_RXM', '<BB',
+                  ['reserved1', 'lpMode']),
+  (CLASS_CFG, MSG_CFG_RST):
+  UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
+  (CLASS_CFG, MSG_CFG_SBAS):
+  UBloxDescriptor('CFG_SBAS', '<BBBBI',
+                  ['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
+  (CLASS_CFG, MSG_CFG_GNSS):
+  UBloxDescriptor('CFG_GNSS', '<BBBB',
+                  ['msgVer', 'numTrkChHw', 'numTrkChUse',
+                   'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
+                  ['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
+  (CLASS_CFG, MSG_CFG_RATE):
+  UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
+  (CLASS_CFG, MSG_CFG_MSG):
+  UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
+  (CLASS_NAV, MSG_NAV_POSLLH):
+  UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
+                  ['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
+  (CLASS_NAV, MSG_NAV_VELNED):
+  UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
+    'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
+  ]),
+  (CLASS_NAV, MSG_NAV_DOP):
+  UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
+                  ['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
+  (CLASS_NAV, MSG_NAV_STATUS):
+  UBloxDescriptor('NAV_STATUS', '<IBBBBII',
+                  ['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
+  (CLASS_NAV, MSG_NAV_SOL):
+  UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
+    'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
+    'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
+  ]),
+  (CLASS_NAV, MSG_NAV_PVT):
+  UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
+    'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
+    'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
+    'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
+    'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
+  ]),
+  (CLASS_NAV, MSG_NAV_POSUTM):
+  UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
+                  ['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
+  (CLASS_NAV, MSG_NAV_SBAS):
+  UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
+    'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
+    'reserved03'
+  ], 'cnt', 'BBBBBBhHh', [
+    'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
+  ]),
+  (CLASS_NAV, MSG_NAV_POSECEF):
+  UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
+  (CLASS_NAV, MSG_NAV_VELECEF):
+  UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
+                                            'sAcc']),
+  (CLASS_NAV, MSG_NAV_TIMEGPS):
+  UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
+                  ['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
+  (CLASS_NAV, MSG_NAV_TIMEUTC):
+  UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
+    'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
+  ]),
+  (CLASS_NAV, MSG_NAV_CLOCK):
+  UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
+  (CLASS_NAV, MSG_NAV_DGPS):
+  UBloxDescriptor('NAV_DGPS', '<IihhBBH',
+                  ['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
+                  'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
+  (CLASS_NAV, MSG_NAV_SVINFO):
+  UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
+                                          'reserved2'], 'numCh', '<BBBBBbhi',
+                  ['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
+  (CLASS_RXM, MSG_RXM_SVSI):
+  UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
+                  '<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
+  (CLASS_RXM, MSG_RXM_EPH):
+  UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
+                  ['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
+  (CLASS_AID, MSG_AID_EPH):
+  UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
+                  ['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
+  (CLASS_AID, MSG_AID_HUI):
+  UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
+                  ['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
+                   'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
+                   'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
+  (CLASS_AID, MSG_AID_AOP):
+  UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
+                  ['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
+                   'optional1[48]']),
+  (CLASS_RXM, MSG_RXM_RAW):
+  UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
+    'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
+  ], 'numMeas', '<ddfBBBBHBBBBBB', [
+    'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'reserved2', 'freqId', 'locktime', 'cno',
+    'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
+  ]),
+  (CLASS_RXM, MSG_RXM_SFRB):
+  UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
+  (CLASS_RXM, MSG_RXM_SFRBX):
+  UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords', 
+      'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
+  (CLASS_AID, MSG_AID_ALM):
+  UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
+  (CLASS_RXM, MSG_RXM_ALM):
+  UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
+  (CLASS_CFG, MSG_CFG_ODO):
+  UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
+    'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
+    'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
+  ]),
+  (CLASS_CFG, MSG_CFG_NAV5):
+  UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
+    'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
+    'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
+    'reserved3', 'reserved4'
+  ]),
+  (CLASS_CFG, MSG_CFG_NAVX5):
+  UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
+    'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
+    'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
+    'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
+    'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
+  ]),
+  (CLASS_MON, MSG_MON_HW):
+  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
+    'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
+    'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
+    'pullL'
+  ]),
+  (CLASS_MON, MSG_MON_HW2):
+  UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
+    'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
+    'reserved2[8]', 'postStatus', 'reserved3[4]'
+  ]),
+  (CLASS_MON, MSG_MON_SCHD):
+  UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
+    'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
+    'ptlSly'
+  ]),
+  (CLASS_MON, MSG_MON_VER):
+  UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
+                  '_remaining', '30s', ['extension']),
+  (CLASS_TIM, MSG_TIM_TP):
+  UBloxDescriptor('TIM_TP', '<IIiHBB',
+                  ['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
+  (CLASS_TIM, MSG_TIM_TM2):
+  UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
+    'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
+    'accEst'
+  ]),
+  (CLASS_TIM, MSG_TIM_SVIN):
+  UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
+    'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
+  ]),
+  (CLASS_INF, MSG_INF_ERROR):
+  UBloxDescriptor('INF_ERR', '<18s', ['str']),
+  (CLASS_INF, MSG_INF_DEBUG):
+  UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
+}
+
+
+class UBloxMessage:
+  '''UBlox message class - holds a UBX binary message'''
+
+  def __init__(self):
+    self._buf = ""
+    self._fields = {}
+    self._recs = []
+    self._unpacked = False
+    self.debug_level = 1
+
+  def __str__(self):
+    '''format a message as a string'''
+    if not self.valid():
+      return 'UBloxMessage(INVALID)'
+    type = self.msg_type()
+    if type in msg_types:
+      return msg_types[type].format(self)
+    return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
+
+  def as_dict(self):
+    '''format a message as a string'''
+    if not self.valid():
+      return 'UBloxMessage(INVALID)'
+    type = self.msg_type()
+    if type in msg_types:
+      return msg_types[type].format(self)
+    return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
+
+  def __getattr__(self, name):
+    '''allow access to message fields'''
+    try:
+      return self._fields[name]
+    except KeyError:
+      if name == 'recs':
+        return self._recs
+      raise AttributeError(name)
+
+  def __setattr__(self, name, value):
+    '''allow access to message fields'''
+    if name.startswith('_'):
+      self.__dict__[name] = value
+    else:
+      self._fields[name] = value
+
+  def have_field(self, name):
+    '''return True if a message contains the given field'''
+    return name in self._fields
+
+  def debug(self, level, msg):
+    '''write a debug message'''
+    if self.debug_level >= 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('<H', self._buf[4:6])
+    return payload_length
+
+  def valid_so_far(self):
+    '''check if the message is valid so far'''
+    if len(self._buf) > 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('<BB', self._buf[-2:])
+    return ck_a == ck_a2 and ck_b == ck_b2
+
+  def needed_bytes(self):
+    '''return number of bytes still needed'''
+    if len(self._buf) < 6:
+      return 8 - len(self._buf)
+    return self.msg_length() + 8 - len(self._buf)
+
+  def valid(self):
+    '''check if a message is valid'''
+    return len(self._buf) >= 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('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
+    msg._buf += payload
+    (ck_a, ck_b) = msg.checksum(msg._buf[2:])
+    msg._buf += struct.pack('<BB', ck_a, ck_b)
+    self.send(msg)
+
+  def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
+    '''configure the solution rate in milliseconds'''
+    payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
+    self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
+
+  def configure_message_rate(self, msg_class, msg_id, rate):
+    '''configure the message rate for a given message'''
+    payload = struct.pack('<BBB', msg_class, msg_id, rate)
+    self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
+
+  def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
+    '''configure a IO port'''
+    if baudrate is None:
+      baudrate = self.baudrate
+    payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
+                          outMask, 0, 0, 0, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
+
+  def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
+    '''configure configuration load/save'''
+    payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
+    self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
+
+  def configure_poll(self, msg_class, msg_id, payload=''):
+    '''poll a configuration message'''
+    self.send_message(msg_class, msg_id, payload)
+
+  def configure_poll_port(self, portID=None):
+    '''poll a port configuration'''
+    if portID is None:
+      self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
+    else:
+      self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
+
+  def configure_min_max_sats(self, min_sats=4, max_sats=32):
+    '''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
+    payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
+                          0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
+
+  def module_reset(self, set, mode):
+    ''' Reset the module for hot/warm/cold start'''
+    payload = struct.pack('<HBB', set, mode, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_RST, payload)

selfdrive/locationd/ubloxd.py

@@ -0,0 +1,286 @@
+#!/usr/bin/env python
+import os
+import serial
+import ublox
+import time
+import datetime
+import struct
+import sys
+from cereal import log
+from common import realtime
+import zmq
+import selfdrive.messaging as messaging
+from selfdrive.services import service_list
+from ephemeris import EphemerisData, GET_FIELD_U
+
+panda = os.getenv("PANDA") is not None   # panda directly connected
+grey = not (os.getenv("EVAL") is not None)     # panda through boardd
+debug = os.getenv("DEBUG") is not None   # debug prints
+print_dB = os.getenv("PRINT_DB") is not None     # print antenna dB
+
+timeout = 1
+dyn_model = 4 # auto model
+baudrate = 460800
+ports = ["/dev/ttyACM0","/dev/ttyACM1"]
+rate = 100 # send new data every 100ms
+
+# which SV IDs we have seen and when we got iono
+svid_seen = {}
+svid_ephemeris = {}
+iono_seen = 0
+
+def configure_ublox(dev):
+  # configure ports  and solution parameters and rate
+  # TODO: configure constellations and channels to allow for 10Hz and high precision
+  dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
+  dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
+
+  if panda:
+    payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
+    dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
+  else:
+    payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
+    dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
+
+  dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
+  dev.configure_poll_port()
+  dev.configure_poll_port(ublox.PORT_SERIAL1)
+  dev.configure_poll_port(ublox.PORT_SERIAL2)
+  dev.configure_poll_port(ublox.PORT_USB)
+  dev.configure_solution_rate(rate_ms=rate)
+
+  # Configure solution
+  payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
+  payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
+                                            3, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
+  #payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0)
+  #dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
+
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
+
+  # Configure RAW and PVT messages to be sent every solution cycle
+  dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
+
+
+
+def int_to_bool_list(num):
+  # for parsing bool bytes
+  return [bool(num & (1<<n)) for n in range(8)]
+
+
+def gen_ephemeris(ephem_data):
+  ephem = {'ephemeris':
+          {'svId': ephem_data.svId,
+
+           'toc': ephem_data.toc,
+           'gpsWeek': ephem_data.gpsWeek,
+
+           'af0': ephem_data.af0,
+           'af1': ephem_data.af1,
+           'af2': ephem_data.af2,
+
+           'iode': ephem_data.iode,
+           'crs': ephem_data.crs,
+           'deltaN': ephem_data.deltaN,
+           'm0': ephem_data.M0,
+
+           'cuc': ephem_data.cuc,
+           'ecc': ephem_data.ecc,
+           'cus': ephem_data.cus,
+           'a': ephem_data.A,
+
+           'toe': ephem_data.toe,
+           'cic': ephem_data.cic,
+           'omega0': ephem_data.omega0,
+           'cis': ephem_data.cis,
+
+           'i0': ephem_data.i0,
+           'crc': ephem_data.crc,
+           'omega': ephem_data.omega,
+           'omegaDot': ephem_data.omega_dot,
+
+           'iDot': ephem_data.idot,
+
+           'tgd': ephem_data.Tgd,
+
+           'ionoCoeffsValid': ephem_data.ionoCoeffsValid,
+           'ionoAlpha': ephem_data.ionoAlpha,
+           'ionoBeta': ephem_data.ionoBeta}}
+  return log.Event.new_message(ubloxGnss=ephem)
+
+
+def gen_solution(msg):
+  msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
+  timestamp = int(((datetime.datetime(msg_data['year'],
+                                      msg_data['month'],
+                                      msg_data['day'],
+                                      msg_data['hour'],
+                                      msg_data['min'],
+                                      msg_data['sec'])
+                 - datetime.datetime(1970,1,1)).total_seconds())*1e+03
+                 + msg_data['nano']*1e-06)
+  gps_fix = {'bearing': msg_data['headMot']*1e-05,  # heading of motion in degrees
+             'altitude': msg_data['height']*1e-03,  # altitude above ellipsoid
+             'latitude': msg_data['lat']*1e-07,  # latitude in degrees
+             'longitude': msg_data['lon']*1e-07,  # longitude in degrees
+             'speed': msg_data['gSpeed']*1e-03,  # ground speed in meters
+             'accuracy': msg_data['hAcc']*1e-03,  # horizontal accuracy (1 sigma?)
+             'timestamp': timestamp,  # UTC time in ms since start of UTC stime 
+             'vNED': [msg_data['velN']*1e-03,
+                     msg_data['velE']*1e-03,
+                     msg_data['velD']*1e-03],  # velocity in NED frame in m/s
+             'speedAccuracy': msg_data['sAcc']*1e-03,  # speed accuracy in m/s
+             'verticalAccuracy': msg_data['vAcc']*1e-03,  # vertical accuracy in meters
+             'bearingAccuracy': msg_data['headAcc']*1e-05,  # heading accuracy in degrees
+             'source': 'ublox'}
+  return log.Event.new_message(gpsLocationExternal=gps_fix)
+
+def gen_nav_data(msg, nav_frame_buffer):
+  # TODO this stuff needs to be parsed and published.
+  # refer to https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_%28UBX-13003221%29.pdf
+  # section 9.1
+  msg_meta_data, measurements = msg.unpack()
+
+  # parse GPS ephem
+  gnssId = msg_meta_data['gnssId']
+  if gnssId  == 0:
+    svId =  msg_meta_data['svid']
+    subframeId =  GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
+    words = []
+    for m in measurements:
+      words.append(m['dwrd'])
+
+    # parse from
+    if subframeId == 1:
+      nav_frame_buffer[gnssId][svId] = {}
+      nav_frame_buffer[gnssId][svId][subframeId] = words
+    elif subframeId-1 in nav_frame_buffer[gnssId][svId]:
+      nav_frame_buffer[gnssId][svId][subframeId] = words
+    if len(nav_frame_buffer[gnssId][svId]) == 5:
+      ephem_data = EphemerisData(svId, nav_frame_buffer[gnssId][svId])
+      return gen_ephemeris(ephem_data)
+
+
+
+
+def gen_raw(msg):
+  # meta data is in first part of tuple
+  # list of measurements is in second part
+  msg_meta_data, measurements = msg.unpack()
+  measurements_parsed = []
+  for m in measurements:
+    trackingStatus_bools = int_to_bool_list(m['trkStat'])
+    trackingStatus = {'pseudorangeValid': trackingStatus_bools[0],
+                      'carrierPhaseValid': trackingStatus_bools[1],
+                      'halfCycleValid': trackingStatus_bools[2],
+                      'halfCycleSubtracted': trackingStatus_bools[3]}
+    measurements_parsed.append({
+        'svId': m['svId'],
+        'pseudorange': m['prMes'],
+        'carrierCycles': m['cpMes'],
+        'doppler': m['doMes'],
+        'gnssId': m['gnssId'],
+        'glonassFrequencyIndex': m['freqId'],
+        'locktime': m['locktime'],
+        'cno': m['cno'],
+        'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
+        'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
+        'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
+        'trackingStatus': trackingStatus})
+  if print_dB:
+    cnos = {}
+    for meas in measurements_parsed:
+      cnos[meas['svId']] = meas['cno']
+    print 'Carrier to noise ratio for each sat: \n', cnos, '\n'
+  receiverStatus_bools = int_to_bool_list(msg_meta_data['recStat'])
+  receiverStatus = {'leapSecValid': receiverStatus_bools[0],
+                    'clkReset': receiverStatus_bools[2]}
+  raw_meas = {'measurementReport': {'rcvTow': msg_meta_data['rcvTow'],
+                'gpsWeek': msg_meta_data['week'],
+                'leapSeconds': msg_meta_data['leapS'],
+                'receiverStatus': receiverStatus,
+                'numMeas': msg_meta_data['numMeas'],
+                'measurements': measurements_parsed}}
+  return log.Event.new_message(ubloxGnss=raw_meas)
+
+def init_reader():
+  port_counter = 0
+  while True:
+    try:
+      dev = ublox.UBlox(ports[port_counter], baudrate=baudrate, timeout=timeout, panda=panda, grey=grey)
+      configure_ublox(dev)
+      return dev
+    except serial.serialutil.SerialException as e:
+      print(e)
+      port_counter = (port_counter + 1)%len(ports)
+      time.sleep(2)
+
+def handle_msg(dev, msg, nav_frame_buffer):
+  try:
+    if debug:
+      print(str(msg))
+      sys.stdout.flush()
+    if msg.name() == 'NAV_PVT':
+      sol = gen_solution(msg)
+      sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      gpsLocationExternal.send(sol.to_bytes())
+    elif msg.name() == 'RXM_RAW':
+      raw = gen_raw(msg)
+      raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      ubloxGnss.send(raw.to_bytes())
+    elif msg.name() == 'RXM_SFRBX':
+      nav = gen_nav_data(msg, nav_frame_buffer)
+      if nav is not None:
+        nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+        ubloxGnss.send(nav.to_bytes())
+
+    else:
+      print "UNKNNOWN MESSAGE:", msg.name()
+  except ublox.UBloxError as e:
+    print(e)
+
+  #if dev is not None and dev.dev is not None:
+  #  dev.close()
+
+def main(gctx=None):
+  global gpsLocationExternal, ubloxGnss
+  nav_frame_buffer = {}
+  nav_frame_buffer[0] = {}
+  for i in xrange(1,33):
+    nav_frame_buffer[0][i] = {}
+
+
+  context = zmq.Context()
+  gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
+  ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
+
+  dev = init_reader()
+  while True:
+    try:
+      msg = dev.receive_message()
+    except serial.serialutil.SerialException as e:
+      print(e)
+      dev.close()
+      dev = init_reader()
+    if msg is not None:
+      handle_msg(dev, msg, nav_frame_buffer)
+
+if __name__ == "__main__":
+  main()

selfdrive/manager.py

@@ -53,7 +53,6 @@ os.environ['BASEDIR'] = BASEDIR
 
 import zmq
 from setproctitle import setproctitle
-from smbus2 import SMBus
 
 from common.params import Params
 from common.realtime import sec_since_boot
@@ -63,7 +62,7 @@ from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
 from selfdrive.thermal import read_thermal
 from selfdrive.registration import register
-from selfdrive.version import version
+from selfdrive.version import version, dirty
 import selfdrive.crash as crash
 
 from selfdrive.loggerd.config import ROOT
@@ -75,6 +74,8 @@ managed_processes = {
   "uploader": "selfdrive.loggerd.uploader",
   "controlsd": "selfdrive.controls.controlsd",
   "radard": "selfdrive.controls.radard",
+  "ubloxd": "selfdrive.locationd.ubloxd",
+  "locationd_dummy": "selfdrive.locationd.locationd_dummy",
   "loggerd": ("selfdrive/loggerd", ["./loggerd"]),
   "logmessaged": "selfdrive.logmessaged",
   "tombstoned": "selfdrive.tombstoned",
@@ -107,6 +108,8 @@ persistent_processes = [
   'uploader',
   'ui',
   'gpsd',
+  'ubloxd',
+  'locationd_dummy',
   'updated',
 ]
 
@@ -231,28 +234,22 @@ def cleanup_all_processes(signal, frame):
 
 # ****************** run loop ******************
 
-def manager_init():
+def manager_init(should_register=True):
   global gctx
 
-  reg_res = register()
+  if should_register:
-  if reg_res:
+    reg_res = register()
-    dongle_id, dongle_secret = reg_res
+    if reg_res:
+      dongle_id, dongle_secret = reg_res
+    else:
+      raise Exception("server registration failed")
   else:
-    raise Exception("server registration failed")
+    dongle_id = "c"*16
 
   # set dongle id
   cloudlog.info("dongle id is " + dongle_id)
   os.environ['DONGLE_ID'] = dongle_id
 
-  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
   cloudlog.info("dirty is %d" % dirty)
   if not dirty:
     os.environ['CLEAN'] = '1'
@@ -286,6 +283,7 @@ def setup_eon_fan():
 
   os.system("echo 2 > /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
+    # have we seen a panda?
-    if passive and not ignition_seen:
+    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:

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

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
 

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())

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)

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

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.