preparing for PRpreparing for PR

.gitignore

@@ -3,7 +3,6 @@
 .ipynb_checkpoints
 .idea
 .sconsign.dblite
-.vscode
 model2.png
 a.out
 
@@ -23,18 +22,13 @@ a.out
 *.vcd
 config.json
 clcache
-launch_chffrplus.sh
-.vscode/*.json
-selfdrive/visiond/visiond
 
-.vscode/
 board/obj/
 selfdrive/boardd/boardd
 selfdrive/logcatd/logcatd
 selfdrive/proclogd/proclogd
 selfdrive/ui/ui
 selfdrive/test/tests/plant/out
-openpilot_tools/
 /src/
 
 one

cereal/car.capnp

@@ -355,6 +355,7 @@ struct CarParams {
   steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
   steerControlType @46 :SteerControlType;
   radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN
+  syncID @51  :Int16;  # SyncID is optional
 
   steerActuatorDelay @48 :Float32; # Steering wheel actuator delay in seconds
   openpilotLongitudinalControl @50 :Bool; # is openpilot doing the longitudinal control?

common/realtime.py

@@ -88,10 +88,12 @@ def remaining(self):
     return self._remaining
 
   # Maintain loop rate by calling this at the end of each loop
-  def keep_time(self):
+  def keep_time(self, offset=False):
     lagged = self.monitor_time()
     if self._remaining > 0:
       time.sleep(self._remaining)
+    elif offset:
+      self._next_frame_time += self._interval / 100
     return lagged
 
   # this only monitor the cumulative lag, but does not enforce a rate
@@ -105,4 +107,3 @@ def monitor_time(self):
     self._frame += 1
     self._remaining = remaining
     return lagged
-

panda/examples/can_logger.py

@@ -2,7 +2,6 @@
 from __future__ import print_function
 import binascii
 import csv
-import time
 import sys
 from panda import Panda
 
@@ -25,25 +24,16 @@ def can_logger():
     outputfile = open('output.csv', 'wb')
     csvwriter = csv.writer(outputfile)
     #Write Header
-    csvwriter.writerow(['Time', 'MessageID', 'Bus', 'Message'])
-    #csvwriter.writerow(['Bus', 'MessageID', 'Message', 'MessageLength'])
+    csvwriter.writerow(['Bus', 'MessageID', 'Message', 'MessageLength'])
     print("Writing csv file output.csv. Press Ctrl-C to exit...\n")
 
     bus0_msg_cnt = 0
     bus1_msg_cnt = 0
     bus2_msg_cnt = 0
-    startTime = 0.0
-    cycle_count = 0
-    p.can_clear(0)
-    p.can_clear(1)
-    p.can_clear(2)
     while True:
-      can_recv = p.can_recv() 
-      cycle_count += 1
+      can_recv = p.can_recv()
       for address, _, dat, src  in can_recv:
-        if startTime == 0.0:
-          startTime = time.time()
-        csvwriter.writerow([str(time.time()-startTime), str(address), str(src), binascii.hexlify(dat)])
+        csvwriter.writerow([str(src), str(hex(address)), "0x" + binascii.hexlify(dat), len(dat)])
 
         if src == 0:
           bus0_msg_cnt += 1
@@ -52,7 +42,7 @@ def can_logger():
         elif src == 2:
           bus2_msg_cnt += 1
 
-        if (bus0_msg_cnt + bus1_msg_cnt + bus2_msg_cnt) % 1000 == 0: print("Message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt), end='\r')
+        print("Message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt), end='\r')
 
   except KeyboardInterrupt:
     print("\nNow exiting. Final message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt))

selfdrive/boardd/boardd.cc

@@ -64,6 +64,10 @@ pthread_t safety_setter_thread_handle = -1;
 pthread_t pigeon_thread_handle = -1;
 bool pigeon_needs_init;
 
+int big_recv;
+uint32_t big_data[RECV_SIZE*2];
+uint16_t sync_id;
+
 void pigeon_init();
 void *pigeon_thread(void *crap);
 
@@ -90,7 +94,8 @@ void *safety_setter_thread(void *s) {
 
   auto safety_model = car_params.getSafetyModel();
   auto safety_param = car_params.getSafetyParam();
-  LOGW("setting safety model: %d with param %d", safety_model, safety_param);
+  sync_id = car_params.getSyncID();
+  LOGW("setting safety model: %d with param %d and sync id %d", safety_model, safety_param, sync_id);
 
   int safety_setting = 0;
   switch (safety_model) {
@@ -212,15 +217,22 @@ void handle_usb_issue(int err, const char func[]) {
   // TODO: check other errors, is simply retrying okay?
 }
 
-void can_recv(void *s) {
+bool can_recv(void *s, uint64_t locked_wake_time, bool force_send) {
   int err;
   uint32_t data[RECV_SIZE/4];
-  int recv;
-  uint32_t f1, f2;
+  int recv, big_index;
+  uint32_t f1, f2, address;
+  bool frame_sent;
+  uint64_t cur_time;
+  frame_sent = false;
 
   // do recv
   pthread_mutex_lock(&usb_lock);
 
+  cur_time = 1e-3 * nanos_since_boot();
+  if (locked_wake_time > cur_time) {
+    usleep(locked_wake_time - cur_time);
+  }
   do {
     err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
     if (err != 0) { handle_usb_issue(err, __func__); }
@@ -234,36 +246,59 @@ void can_recv(void *s) {
 
   // return if length is 0
   if (recv <= 0) {
-    return;
+    return false;
   }
 
-  // create message
-  capnp::MallocMessageBuilder msg;
-  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
-  event.setLogMonoTime(nanos_since_boot());
-
-  auto canData = event.initCan(recv/0x10);
-
-  // populate message
+  big_index = big_recv/0x10;
   for (int i = 0; i<(recv/0x10); i++) {
+    big_data[(big_index + i)*4] = data[i*4];
+    big_data[(big_index + i)*4+1] = data[i*4+1];
+    big_data[(big_index + i)*4+2] = data[i*4+2];
+    big_data[(big_index + i)*4+3] = data[i*4+3];
+    big_recv += 0x10;
     if (data[i*4] & 4) {
       // extended
-      canData[i].setAddress(data[i*4] >> 3);
-      //printf("got extended: %x\n", data[i*4] >> 3);
+      address = data[i*4] >> 3;
+      //printf("got extended: %x\n", big_data[i*4] >> 3);
     } else {
       // normal
-      canData[i].setAddress(data[i*4] >> 21);
+      address = data[i*4] >> 21;
     }
-    canData[i].setBusTime(data[i*4+1] >> 16);
-    int len = data[i*4+1]&0xF;
-    canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
-    canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
+    if (address == sync_id) force_send = true;
   }
+  if (force_send) {
+    frame_sent = true;
 
-  // send to can
-  auto words = capnp::messageToFlatArray(msg);
-  auto bytes = words.asBytes();
-  zmq_send(s, bytes.begin(), bytes.size(), 0);
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+
+    auto can_data = event.initCan(big_recv/0x10);
+
+    // populate message
+    for (int i = 0; i<(big_recv/0x10); i++) {
+      if (big_data[i*4] & 4) {
+        // extended
+        can_data[i].setAddress(big_data[i*4] >> 3);
+        //printf("got extended: %x\n", big_data[i*4] >> 3);
+      } else {
+        // normal
+        can_data[i].setAddress(big_data[i*4] >> 21);
+      }
+      can_data[i].setBusTime(big_data[i*4+1] >> 16);
+      int len = big_data[i*4+1]&0xF;
+      can_data[i].setDat(kj::arrayPtr((uint8_t*)&big_data[i*4+2], len));
+      can_data[i].setSrc((big_data[i*4+1] >> 4) & 0xff);
+    }
+
+    // send to can
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    zmq_send(s, bytes.begin(), bytes.size(), 0);
+    big_recv = 0;
+  }
+
+  return frame_sent;
 }
 
 void can_health(void *s) {
@@ -447,11 +482,29 @@ void *can_recv_thread(void *crap) {
   void *publisher = zmq_socket(context, ZMQ_PUB);
   zmq_bind(publisher, "tcp://*:8006");
 
-  // run at ~200hz
+  bool frame_sent, skip_once, force_send;
+  uint64_t wake_time, locked_wake_time, cur_time;
+  force_send = true;
+  int sleepTime;
+  cur_time = 1e-3 * nanos_since_boot();
+  wake_time = cur_time;
+
   while (!do_exit) {
-    can_recv(publisher);
-    // 5ms
-    usleep(5*1000);
+
+    frame_sent = can_recv(publisher, locked_wake_time, force_send);
+    if (frame_sent == true || skip_once == true) {
+      cur_time = 1e-3 * nanos_since_boot();
+      skip_once = frame_sent;
+      wake_time += 4500;
+      if (cur_time < wake_time) {
+        usleep(wake_time - cur_time);
+      }
+    }
+    else {
+      force_send = (sync_id == 0);
+      wake_time += 1000;
+      locked_wake_time = wake_time;
+    }
   }
   return NULL;
 }

selfdrive/car/chrysler/interface.py

@@ -138,7 +138,7 @@ def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
 
-    self.cp.update(int(sec_since_boot() * 1e9), False)
+    self.cp.update(int(sec_since_boot() * 1e9), True)
 
     self.CS.update(self.cp)
 

selfdrive/car/ford/interface.py

@@ -138,7 +138,7 @@ def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
 
-    self.cp.update(int(sec_since_boot() * 1e9), False)
+    self.cp.update(int(sec_since_boot() * 1e9), True)
 
     self.CS.update(self.cp)
 

selfdrive/car/gm/interface.py

@@ -196,7 +196,7 @@ def get_params(candidate, fingerprint):
   # returns a car.CarState
   def update(self, c):
 
-    self.pt_cp.update(int(sec_since_boot() * 1e9), False)
+    self.pt_cp.update(int(sec_since_boot() * 1e9), True)
     self.CS.update(self.pt_cp)
 
     # create message

selfdrive/car/honda/interface.py

@@ -366,7 +366,7 @@ def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
 
-    self.cp.update(int(sec_since_boot() * 1e9), False)
+    self.cp.update(int(sec_since_boot() * 1e9), True)
     self.cp_cam.update(int(sec_since_boot() * 1e9), False)
 
     self.CS.update(self.cp, self.cp_cam)

selfdrive/car/hyundai/interface.py

@@ -183,7 +183,7 @@ def get_params(candidate, fingerprint):
   def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
-    self.cp.update(int(sec_since_boot() * 1e9), False)
+    self.cp.update(int(sec_since_boot() * 1e9), True)
     self.cp_cam.update(int(sec_since_boot() * 1e9), False)
     self.CS.update(self.cp, self.cp_cam)
     # create message

selfdrive/car/toyota/interface.py

@@ -219,7 +219,7 @@ def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
 
-    self.cp.update(int(sec_since_boot() * 1e9), False)
+    self.cp.update(int(sec_since_boot() * 1e9), True)
 
     # run the cam can update for 10s as we just need to know if the camera is alive
     if self.frame < 1000:

selfdrive/controls/controlsd.py

@@ -459,7 +459,7 @@ def controlsd_thread(gctx=None, rate=100):
   path_plan = messaging.new_message()
   path_plan.init('pathPlan')
 
-  rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
+  rk = Ratekeeper(rate, print_delay_threshold=0.2 / 1000)
   controls_params = params.get("ControlsParams")
   # Read angle offset from previous drive
   if controls_params is not None:
@@ -503,14 +503,14 @@ def controlsd_thread(gctx=None, rate=100):
                     v_cruise_kph_last, AM, rk, driver_status,
                     LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
 
+    rk.keep_time(True)  # Run at 100Hz
     prof.checkpoint("State Control")
 
     # Publish data
     CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
                    live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
     prof.checkpoint("Sent")
 
-    rk.keep_time()  # Run at 100Hz
     prof.display()