Synchronize controls with the CAN bus

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?

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,23 @@ 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) {
+    // Short sleep occurs after usb_lock to ensure sync timing
+    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__); }
@@ -232,38 +245,61 @@ void can_recv(void *s) {
 
   pthread_mutex_unlock(&usb_lock);
 
-  // return if length is 0
-  if (recv <= 0) {
-    return;
+  // return if both buffers are empty
+  if ((big_recv <= 0) && (recv <= 0)) {
+    return true;
   }
 
-  // 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 +483,89 @@ 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, last_long_sleep;
+  int recv_state = 0;
+  force_send = true;
+  last_long_sleep = 1e-3 * nanos_since_boot();
+  wake_time = last_long_sleep;
+  locked_wake_time = wake_time;
+
   while (!do_exit) {
-    can_recv(publisher);
-    // 5ms
-    usleep(5*1000);
+    while (sync_id > 0 && !do_exit) {
+      frame_sent = can_recv(publisher, locked_wake_time, force_send);
+
+      // drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set)
+      if (frame_sent == true || skip_once == true) {
+        last_long_sleep = 1e-3 * nanos_since_boot();
+        skip_once = frame_sent;
+        wake_time += 4500;
+        force_send = false;
+        if (last_long_sleep < wake_time) {
+          usleep(wake_time - last_long_sleep);
+        }
+        else {
+          if ((last_long_sleep - wake_time) > 5e5) {
+            // probably a new drive
+            wake_time = last_long_sleep;
+          }
+          else {
+            if (skip_once) {
+              wake_time += 4500;
+              skip_once = false;
+              if (last_long_sleep < wake_time) {
+                usleep(wake_time - last_long_sleep);
+              }
+              else {
+                printf("   lagging sync %d \n", sync_id);
+              }
+            }
+          }
+        }
+      }
+      else {
+        //force_send = (locked_wake_time > last_long_sleep);
+        wake_time += 1000;
+        locked_wake_time = wake_time;
+      }
+    }
+    while (sync_id == 0 && !do_exit) {
+      frame_sent = can_recv(publisher, locked_wake_time, force_send);
+
+      // drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set)
+      if (recv_state++ < 2) {
+        last_long_sleep = 1e-3 * nanos_since_boot();
+        wake_time += 4500;
+        force_send = false;
+        if (last_long_sleep < wake_time) {
+          usleep(wake_time - last_long_sleep);
+        }
+        else {
+          if ((last_long_sleep - wake_time) > 5e5) {
+            // probably a new drive
+            wake_time = last_long_sleep;
+          }
+          else {
+            if (recv_state < 2) {
+              wake_time += 4500;
+              recv_state++;
+              if (last_long_sleep < wake_time) {
+                usleep(wake_time - last_long_sleep);
+              }
+              else {
+                printf("    lagging!\n");
+              }
+            }
+          }
+        }
+      }
+      else {
+        force_send = true;
+        recv_state = 0;
+        wake_time += 1000;
+        locked_wake_time = wake_time;
+      }
+    }
   }
   return NULL;
 }
@@ -635,7 +749,7 @@ int main() {
   LOGW("starting boardd");
 
   // set process priority
-  err = set_realtime_priority(4);
+  err = set_realtime_priority(3);
   LOG("setpriority returns %d", err);
 
   // check the environment

selfdrive/can/parser.cc

@@ -303,15 +303,19 @@ class CANParser {
 
     // multiple recv is fine
     bool first = wait;
-    while (1) {
+    while (first || drain) {
       if (first) {
         err = zmq_msg_recv(&msg, subscriber, 0);
         first = false;
       } else {
+        // Drain the queue at startup
+        usleep(500);
         err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
       }
-      if (err < 0) break;
-
+      if (err < 0) {
+        drain = false;
+        break;
+      }
       // format for board, make copy due to alignment issues, will be freed on out of scope
       auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
       memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
@@ -355,6 +359,7 @@ class CANParser {
 
  private:
   const int bus;
+  bool drain = true;
   // zmq vars
   void *context = NULL;
   void *subscriber = 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

@@ -194,6 +194,7 @@ def get_params(candidate, fingerprint):
       ret.centerToFront = centerToFront_civic
       ret.steerRatio = 14.63  # 10.93 is end-to-end spec
       tire_stiffness_factor = 1.
+      ret.syncID = 330
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
       ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
@@ -214,6 +215,7 @@ def get_params(candidate, fingerprint):
       ret.centerToFront = ret.wheelbase * 0.39
       ret.steerRatio = 15.96  # 11.82 is spec end-to-end
       tire_stiffness_factor = 0.8467
+      ret.syncID = 330
       ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -224,6 +226,7 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.mass = 3095 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.67
+      ret.syncID = 342
       ret.centerToFront = ret.wheelbase * 0.37
       ret.steerRatio = 18.61  # 15.3 is spec end-to-end
       tire_stiffness_factor = 0.72
@@ -237,6 +240,7 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.mass = 3572 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.62
+      ret.syncID = 330
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.3         # as spec
       tire_stiffness_factor = 0.444 # not optimized yet
@@ -248,6 +252,7 @@ def get_params(candidate, fingerprint):
 
     elif candidate == CAR.CRV_5G:
       stop_and_go = True
+      ret.syncID = 342
       ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
       ret.mass = 3410. * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.66
@@ -264,6 +269,7 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.mass = 3935 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.68
+      ret.syncID = 342
       ret.centerToFront = ret.wheelbase * 0.38
       ret.steerRatio = 15.0         # as spec
       tire_stiffness_factor = 0.444 # not optimized yet
@@ -277,6 +283,7 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.mass = 4471 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 3.00
+      ret.syncID = 342
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 14.35        # as spec
       tire_stiffness_factor = 0.82
@@ -290,6 +297,7 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.mass = 4303 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.81
+      ret.syncID = 342
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 16.0         # as spec
       tire_stiffness_factor = 0.444 # not optimized yet
@@ -302,6 +310,7 @@ def get_params(candidate, fingerprint):
     elif candidate == CAR.RIDGELINE:
       stop_and_go = False
       ret.mass = 4515 * CV.LB_TO_KG + std_cargo
+      ret.syncID = 342
       ret.wheelbase = 3.18
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.59        # as spec
@@ -366,7 +375,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: