openpilot v0.4.0.2 release

old-commit-hash: da52d065a4

RELEASES.md

@@ -1,3 +1,8 @@
+Version 0.4.0.2 (2018-01-18)
+==========================
+ * Add focus adjustment slider
+ * Minor bugfixes
+
 Version 0.4.0.1 (2017-12-21)
 ==========================
  * New UI to match chffrplus

apk/ai.comma.plus.offroad.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d6f5543949481e6c9dc225d33efb122957a8a85b2c16e6454f8c4f1b01d5d463
-size 15385239
+oid sha256:3ee3fcae5727530b008d40adf962bb5fac2f7c2b42745408b65f316c112905a6
+size 15385883

cereal/log.capnp

@@ -33,6 +33,7 @@ struct InitData {
   androidBuildInfo @5 :AndroidBuildInfo;
   androidSensors @6 :List(AndroidSensor);
   chffrAndroidExtra @7 :ChffrAndroidExtra;
+  iosBuildInfo @14 :IosBuildInfo;
 
   pandaInfo @8 :PandaInfo;
 
@@ -43,6 +44,7 @@ struct InitData {
     unknown @0;
     neo @1;
     chffrAndroid @2;
+    chffrIos @3;
   }
 
   struct AndroidBuildInfo {
@@ -93,6 +95,13 @@ struct InitData {
     allCameraCharacteristics @0 :Map(Text, Text);
   }
 
+  struct IosBuildInfo {
+    appVersion @0 :Text;
+    appBuild @1 :UInt32;
+    osVersion @2 :Text;
+    deviceModel @3 :Text;
+  }
+
   struct PandaInfo {
     hasPanda @0 :Bool;
     dongleId @1 :Text;
@@ -222,6 +231,7 @@ struct GpsLocationData {
     fusion @4;
     external @5;
     ublox @6;
+    trimble @7;
   }
 }
 
@@ -374,6 +384,8 @@ struct Live100Data {
 
   angleOffset @27 :Float32;
 
+  gpsPlannerActive @40 :Bool;
+
   enum ControlState {
     disabled @0;
     preEnabled @1;
@@ -514,6 +526,8 @@ struct Plan {
   # gps trajectory in car frame
   gpsTrajectory @12 :GpsTrajectory;
 
+  gpsPlannerActive @19 :Bool;
+
   struct GpsTrajectory {
     x @0 :List(Float32);
     y @1 :List(Float32);
@@ -1298,6 +1312,27 @@ struct LiveLongitudinalMpcData {
   calculationTime @9 :UInt64;
 }
 
+
+struct ECEFPoint {
+  x @0 :Float32;
+  y @1 :Float32;
+  z @2 :Float32;
+}
+
+struct GPSPlannerPoints {
+  curPos @0 :ECEFPoint;
+  points @1 :List(ECEFPoint);
+  valid @2 :Bool;
+  trackName @3 :Text;
+  instructionProgress @4 :Float32;
+}
+
+struct GPSPlannerPlan {
+  valid @0 :Bool;
+  poly @1 :List(Float32);
+  trackName @2 :Text;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -1341,5 +1376,8 @@ struct Event {
     liveMpc @36 :LiveMpcData;
     liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
     navStatus @38 :NavStatus;
+    ubloxRaw @39 :Data;
+    gpsPlannerPoints @40 :GPSPlannerPoints;
+    gpsPlannerPlan @41 :GPSPlannerPlan;
   }
 }

requirements_openpilot.txt

@@ -11,4 +11,5 @@ simplejson==3.8.2
 pyyaml==3.12
 cffi==1.7.0
 enum34==1.1.1
+smbus2==0.2.0
 -e git+https://github.com/commaai/le_python.git#egg=Logentries

selfdrive/boardd/boardd.cc

@@ -25,6 +25,8 @@
 #include "common/swaglog.h"
 #include "common/timing.h"
 
+#include <algorithm>
+
 // double the FIFO size
 #define RECV_SIZE (0x1000)
 #define TIMEOUT 0
@@ -45,6 +47,7 @@ pthread_mutex_t usb_lock;
 bool spoofing_started = false;
 bool fake_send = false;
 bool loopback_can = false;
+bool has_pigeon = false;
 
 pthread_t safety_setter_thread_handle = -1;
 
@@ -102,6 +105,8 @@ void *safety_setter_thread(void *s) {
   return NULL;
 }
 
+void pigeon_init();
+
 // must be called before threads or with mutex
 bool usb_connect() {
   int err;
@@ -140,6 +145,8 @@ bool usb_connect() {
     err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
   }
 
+  if (has_pigeon) pigeon_init();
+
   return true;
 fail:
   return false;
@@ -409,6 +416,100 @@ void *can_health_thread(void *crap) {
   return NULL;
 }
 
+#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1)
+
+void _pigeon_send(const char *dat, int len) {
+  int sent;
+  unsigned char a[0x20];
+  int err;
+  a[0] = 1;
+  for (int i=0; i<len; i+=0x1f) {
+    int ll = std::max(0x1f, len-i);
+    memcpy(&a[1], &dat[i], ll);
+    err = libusb_bulk_transfer(dev_handle, 2, a, ll+1, &sent, TIMEOUT);
+  }
+}
+
+void pigeon_set_baud(int baud) {
+  libusb_control_transfer(dev_handle, 0xc0, 0xe4, 1, baud/300, NULL, 0, TIMEOUT);
+}
+
+void pigeon_init() {
+  // power on pigeon
+  libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
+  usleep(100*1000);
+  libusb_control_transfer(dev_handle, 0xc0, 0xd9, 1, 0, NULL, 0, TIMEOUT);
+  usleep(500*1000);
+
+  // baud rate upping
+  pigeon_set_baud(9600);
+  pigeon_send("$PUBX,41,1,0007,0003,230400,0*1A\r\n");
+  usleep(200*1000);
+
+  // set baud rate to 230400
+  pigeon_set_baud(230400);
+
+  // init from ubloxd
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F");
+  pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x00\x00\x00\x00\x00\x00\x00\x00\xF2\x72");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85");
+  pigeon_send("\xB5\x62\x06\x00\x00\x00\x06\x18");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24");
+  pigeon_send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10");
+  pigeon_send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63");
+  pigeon_send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37");
+  pigeon_send("\xB5\x62\x06\x24\x00\x00\x2A\x84");
+  pigeon_send("\xB5\x62\x06\x23\x00\x00\x29\x81");
+  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");
+
+  LOGW("pigeon is ready to fly");
+
+}
+
+void *pigeon_thread(void *crap) {
+  // ubloxRaw = 8042
+  void *context = zmq_ctx_new();
+  void *publisher = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(publisher, "tcp://*:8042");
+
+  // run at ~200hz
+  unsigned char dat[0x40];
+  while (!do_exit) {
+    while (1) {
+      pthread_mutex_lock(&usb_lock);
+      int len = libusb_control_transfer(dev_handle, 0xc0, 0xe0, 1, 0, dat, 0x40, TIMEOUT);
+      pthread_mutex_unlock(&usb_lock);
+
+      if (len <= 0) break;
+
+      // create message
+      capnp::MallocMessageBuilder msg;
+      cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+      event.setLogMonoTime(nanos_since_boot());
+      auto ublox_raw = event.initUbloxRaw(len);
+      memcpy(ublox_raw.begin(), dat, len);
+
+      // send to ubloxRaw
+      auto words = capnp::messageToFlatArray(msg);
+      auto bytes = words.asBytes();
+      zmq_send(publisher, bytes.begin(), bytes.size(), 0);
+
+      if (len < 0x40) break;
+    }
+
+    // 10ms
+    usleep(10*1000);
+  }
+
+  return NULL;
+}
+
 int set_realtime_priority(int level) {
   // should match python using chrt
   struct sched_param sa;
@@ -436,10 +537,14 @@ int main() {
     fake_send = true;
   }
 
-  if(getenv("BOARDD_LOOPBACK")){
+  if (getenv("BOARDD_LOOPBACK")){
     loopback_can = true;
   }
 
+  if (getenv("PIGEON")) {
+    has_pigeon = true;
+  }
+
   // init libusb
   err = libusb_init(&ctx);
   assert(err == 0);
@@ -470,6 +575,15 @@ int main() {
                        thermal_thread, NULL);
   assert(err == 0);
 
+  if (has_pigeon) {
+    pthread_t pigeon_thread_handle;
+    err = pthread_create(&pigeon_thread_handle, NULL,
+                         pigeon_thread, NULL);
+    assert(err == 0);
+    err = pthread_join(pigeon_thread_handle, NULL);
+    assert(err == 0);
+  }
+
   // join threads
 
   err = pthread_join(thermal_thread_handle, NULL);

selfdrive/common/version.h

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

selfdrive/controls/controlsd.py

@@ -210,7 +210,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
   return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
 
 
-def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, 
+def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                   awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
   # Given the state, this function returns the actuators
 
@@ -387,6 +387,9 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
   # log learned angle offset
   dat.live100.angleOffset = float(angle_offset)
 
+  # Save GPS planner status
+  dat.live100.gpsPlannerActive = plan.gpsPlannerActive
+
   # lag
   dat.live100.cumLagMs = -rk.remaining*1000.
 
@@ -513,7 +516,7 @@ def controlsd_thread(gctx, rate=100):
 
     if not passive:
       # update control state
-      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer, 
+      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer,
                                                                                     v_cruise_kph, AM)
       prof.checkpoint("State transition")
 

selfdrive/controls/lib/planner.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import os
 import zmq
 
 import numpy as np
@@ -25,6 +26,8 @@ AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distract
 _DEBUG = False
 _LEAD_ACCEL_TAU = 1.5
 
+GPS_PLANNER_ADDR = "192.168.5.1"
+
 # lookup tables VS speed to determine min and max accels in cruise
 # make sure these accelerations are smaller than mpc limits
 _A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
@@ -258,6 +261,12 @@ class Planner(object):
     self.poller = zmq.Poller()
     self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller)
     self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller)
+
+    if os.environ.get('GPS_PLANNER_ACTIVE', False):
+      self.gps_planner_plan = messaging.sub_sock(context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR)
+    else:
+      self.gps_planner_plan = None
+
     self.plan = messaging.pub_sock(context, service_list['plan'].port)
     self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
 
@@ -292,6 +301,9 @@ class Planner(object):
     self.fcw_checker = FCWChecker()
     self.fcw_enabled = fcw_enabled
 
+    self.last_gps_planner_plan = None
+    self.gps_planner_active = False
+
   def choose_solution(self, v_cruise_setpoint, enabled):
     if enabled:
       solutions = {'cruise': self.v_cruise}
@@ -329,12 +341,18 @@ class Planner(object):
 
     md = None
     l20 = None
+    gps_planner_plan = None
 
     for socket, event in self.poller.poll(0):
       if socket is self.model:
         md = messaging.recv_one(socket)
       elif socket is self.live20:
         l20 = messaging.recv_one(socket)
+      elif socket is self.gps_planner_plan:
+        gps_planner_plan = messaging.recv_one(socket)
+
+    if gps_planner_plan is not None:
+      self.last_gps_planner_plan = gps_planner_plan
 
     if md is not None:
       self.last_md_ts = md.logMonoTime
@@ -343,6 +361,17 @@ class Planner(object):
 
       self.PP.update(CS.vEgo, md)
 
+      if self.last_gps_planner_plan is not None:
+        plan = self.last_gps_planner_plan.gpsPlannerPlan
+        self.gps_planner_active = plan.valid
+        if plan.valid:
+          self.PP.d_poly = plan.poly
+          self.PP.p_poly = plan.poly
+          self.PP.c_poly = plan.poly
+          self.PP.l_prob = 0.0
+          self.PP.r_prob = 0.0
+          self.PP.c_prob = 1.0
+
     if l20 is not None:
       self.last_l20_ts = l20.logMonoTime
       self.last_l20 = cur_time
@@ -454,6 +483,8 @@ class Planner(object):
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 
+    plan_send.plan.gpsPlannerActive = self.gps_planner_active
+
     # Send out fcw
     fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
     plan_send.plan.fcw = fcw

selfdrive/debug/dump.py

@@ -13,6 +13,7 @@ if __name__ == "__main__":
   poller = zmq.Poller()
 
   parser = argparse.ArgumentParser(description='Sniff a communcation socket')
+  parser.add_argument('--pipe', action='store_true')
   parser.add_argument('--raw', action='store_true')
   parser.add_argument('--json', action='store_true')
   parser.add_argument('--addr', default='127.0.0.1')
@@ -33,7 +34,10 @@ if __name__ == "__main__":
     for sock, mode in polld:
       if mode != zmq.POLLIN:
         continue
-      if args.raw:
+      if args.pipe:
+        sys.stdout.write(sock.recv())
+        sys.stdout.flush()
+      elif args.raw:
         hexdump(sock.recv())
       elif args.json:
         print(json.loads(sock.recv()))

selfdrive/debug/get_fingerprint.py

@@ -1,14 +1,13 @@
 #!/usr/bin/env python
 
 # simple script to get a vehicle fingerprint.
-# keep this script running for few seconds: some messages are published every few seconds
 
 # Instructions:
 # - connect to a Panda
 # - run selfdrive/boardd/boardd
 # - launching this script
-# - since some messages are published at low frequency, keep this script running for few seconds, 
-#   until all messages are received at least once
+# - since some messages are published at low frequency, keep this script running for few
+#   seconds, until all messages are received at least once
 
 import zmq
 import selfdrive.messaging as messaging

selfdrive/loggerd/loggerd

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

selfdrive/manager.py

@@ -90,6 +90,7 @@ managed_processes = {
   "sensord": ("selfdrive/sensord", ["./sensord"]),
   "gpsd": ("selfdrive/sensord", ["./gpsd"]),
   "updated": "selfdrive.updated",
+  #"gpsplanner": "selfdrive.controls.gps_plannerd",
 }
 
 running = {}
@@ -119,6 +120,7 @@ car_started_processes = [
   'radard',
   'visiond',
   'proclogd',
+  # 'gpsplanner,
 ]
 
 def register_managed_process(name, desc, car_started=False):

selfdrive/sensord/gpsd

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

selfdrive/sensord/sensord

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

selfdrive/service_list.yaml

@@ -48,8 +48,15 @@ liveMpc: [8035, true]
 liveLongitudinalMpc: [8036, true]
 plusFrame: [8037, false]
 navStatus: [8038, true]
+gpsLocationTrimble: [8039, true]
+trimbleGnss: [8041, true]
+ubloxRaw: [8042, true]
+gpsPlannerPoints: [8043, true]
+gpsPlannerPlan: [8044, true]
 
 testModel: [8040, false]
+testLiveLocation: [8045, false]
+
 
 # manager -- base process to manage starting and stopping of all others
 #   subscribes: health
@@ -59,7 +66,7 @@ testModel: [8040, false]
 
 # boardd -- communicates with the car
 #   subscribes: sendcan
-#   publishes:  can, health
+#   publishes:  can, health, ubloxRaw
 
 # sensord -- publishes the IMU and GPS
 #   publishes:  sensorEvents, gpsNMEA

selfdrive/ui/ui.c

@@ -109,6 +109,11 @@ typedef struct UIScene {
   // Used to display calibration progress
   int cal_status;
   int cal_perc;
+
+
+  // Used to show gps planner status
+  bool gps_planner_active;
+
 } UIScene;
 
 typedef struct UIState {
@@ -421,6 +426,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
       .front_box_width = ui_info.front_box_width,
       .front_box_height = ui_info.front_box_height,
       .world_objects_visible = false,  // Invisible until we receive a calibration message.
+      .gps_planner_active = false,
   };
 
   s->rgb_width = back_bufs.width;
@@ -879,6 +885,21 @@ static void ui_draw_vision(UIState *s) {
       } else {
         nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL);
       }
+    } else if (scene->gps_planner_active) {
+      int rec_width = 1120;
+      int x_pos = 500;
+      nvgBeginPath(s->vg);
+      nvgStrokeWidth(s->vg, 14);
+      nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
+      nvgStroke(s->vg);
+      nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
+      nvgFill(s->vg);
+
+      nvgFontSize(s->vg, 40*2.5);
+      nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
+      nvgFontFace(s->vg, "sans-semibold");
+      nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
+      nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL);
     }
   }
 
@@ -1361,6 +1382,7 @@ static void ui_update(UIState *s) {
         s->scene.v_ego = datad.vEgo;
         s->scene.curvature = datad.curvature;
         s->scene.engaged = datad.enabled;
+        s->scene.gps_planner_active = datad.gpsPlannerActive;
         // printf("recv %f\n", datad.vEgo);
 
         s->scene.frontview = datad.rearViewCam;

selfdrive/visiond/visiond

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:06b2290dd088740e5ba460f1d5bff98187530c6bf696518766ad58b9d1aa90ab
-size 13337952
+oid sha256:37fb4201c85e93d198a339ee660120a69cf5ca00760c91ce28b7f450393fc5ac
+size 13341920