gernby 510 merge

cereal/car.capnp

@@ -347,8 +347,9 @@ struct CarParams {
   longitudinalKpV @37 :List(Float32);
   longitudinalKiBP @38 :List(Float32);
   longitudinalKiV @39 :List(Float32);
-
   steerLimitAlert @29 :Bool;
+  steerMPCReactTime @51 :Float32;
+  steerMPCDampTime @52 :Float32;
 
   vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
   directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake

cereal/log.capnp

@@ -397,6 +397,10 @@ struct Live100Data {
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;     # Steering angle in degrees.
   angleSteersDes @29 :Float32;
+  dampAngleSteersDes @52 :Float32;
+  dampRateSteersDes @53 :Float32;
+  rateModeFF @54 :Float32;
+  angleModeFF @55 :Float32;
   curvature @37 :Float32;       # path curvature from vehicle model
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
@@ -613,6 +617,10 @@ struct PathPlan {
 
   angleSteers @8 :Float32; # deg
   rateSteers @13 :Float32; # deg/s
+  mpcAngles @14 :List(Float32);
+  mpcRates @15 :List(Float32);
+  mpcTimes @16 :List(Float32);
+  laneProb @17 :Float32;
   valid @9 :Bool;
   paramsValid @10 :Bool;
   modelValid @12 :Bool;
@@ -1612,6 +1620,7 @@ struct LiveParametersData {
   angleOffsetAverage @3 :Float32;
   stiffnessFactor @4 :Float32;
   steerRatio @5 :Float32;
+  laneWidth @6 :Float32;
 }
 
 struct LiveMapData {

dashfile.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+import csv
+import zmq
+import time
+import numpy as np
+import selfdrive.messaging as messaging
+from selfdrive.services import service_list
+from common.realtime import set_realtime_priority, Ratekeeper
+import os, os.path
+
+# Polling rate should be twice the data rate to prevent aliasing
+def main(rate=100):
+  set_realtime_priority(5)
+  context = zmq.Context()
+  poller = zmq.Poller()
+
+  live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=False, poller=poller)
+  carState = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
+  can = None #messaging.sub_sock(context, service_list['can'].port, conflate=True, poller=poller)
+
+  vEgo = 0.0
+  _live100 = None
+  _can = None
+
+  frame_count = 0
+  skipped_count = 0
+
+  rk = Ratekeeper(rate, print_delay_threshold=np.inf)
+
+  # simple version for working with CWD
+  #print len([name for name in os.listdir('.') if os.path.isfile(name)])
+
+  # path joining version for other paths
+  DIR = '/sdcard/tuning'
+  filenumber = len([name for name in os.listdir(DIR) if os.path.isfile(os.path.join(DIR, name))])
+
+  kegman_counter = 0
+  monoTimeOffset = 0
+  receiveTime = 0
+  angle_rate = 0.0
+
+  print("start")
+  with open(DIR + '/dashboard_file_%d.csv' % filenumber, mode='w') as dash_file:
+    print("opened")
+    dash_writer = csv.writer(dash_file, delimiter=',', quotechar='', quoting=csv.QUOTE_NONE)
+    print("initialized")
+    dash_writer.writerow(['ff_rate','ff_angle', 'angle_steers_des','angle_steers','angle_rate','dampened_angle_steers_des','dampened_angle_rate_des','dampened_angle_steers','v_ego','steer_override','p','i','f','time'])
+    print("first row")
+
+    while 1:
+      for socket, event in poller.poll(0):
+        if socket is can:
+          _can = messaging.recv_one(socket)
+          print(_can)
+
+        if socket is carState:
+          _carState = messaging.drain_sock(socket)
+          for cs in _carState:
+            angle_rate = cs.carState.steeringRate
+
+        if socket is live100:
+          _live100 = messaging.drain_sock(socket)
+          for l100 in _live100:
+            vEgo = l100.live100.vEgo
+            if vEgo > 0: # and l100.live100.active:
+              receiveTime = int(monoTimeOffset + l100.logMonoTime)
+              if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000):
+                monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime
+                receiveTime = int(monoTimeOffset + l100.logMonoTime)
+
+              frame_count += 1
+              dash_writer.writerow([str(round(l100.live100.rateModeFF, 2)),
+                                    str(round(l100.live100.angleModeFF, 2)),
+                                    str(round(l100.live100.angleSteersDes, 2)),
+                                    str(round(l100.live100.angleSteers, 2)),
+                                    str(round(angle_rate, 2)),
+                                    str(round(l100.live100.dampAngleSteersDes, 2)),
+                                    str(round(l100.live100.dampAngleRateDes, 2)),
+                                    str(round(l100.live100.dampAngleSteers, 2)),
+                                    str(round(l100.live100.vEgo, 1)),
+                                    1 if l100.live100.steerOverride else 0,
+                                    str(round(l100.live100.upSteer, 4)),
+                                    str(round(l100.live100.uiSteer, 4)),
+                                    str(round(l100.live100.ufSteer, 4)),
+                                    str(receiveTime)])
+          else:
+            skipped_count += 1
+        else:
+          skipped_count += 1
+      if frame_count % 200 == 0:
+        print("captured = %d" % frame_count)
+        frame_count += 1
+      if skipped_count % 200 == 0:
+        print("skipped = %d" % skipped_count)
+        skipped_count += 1
+
+      rk.keep_time()
+
+if __name__ == "__main__":
+  main()

launch_chffrplus.sh

@@ -6,12 +6,14 @@ 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

selfdrive/boardd/boardd.cc

@@ -65,6 +65,9 @@ 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];
+
 void pigeon_init();
 void *pigeon_thread(void *crap);
 
@@ -216,11 +219,14 @@ 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, 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);
@@ -236,38 +242,52 @@ 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());
+  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 (force_send) {
+    frame_sent = true;
 
-  auto canData = event.initCan(recv/0x10);
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
 
-  // populate message
-  for (int i = 0; i<(recv/0x10); i++) {
-    if (data[i*4] & 4) {
-      // extended
-      canData[i].setAddress(data[i*4] >> 3);
-      //printf("got extended: %x\n", data[i*4] >> 3);
-    } else {
-      // normal
-      canData[i].setAddress(data[i*4] >> 21);
+    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);
     }
-    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);
+
+    // send to can
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    zmq_send(s, bytes.begin(), bytes.size(), 0);
+    big_recv = 0;
   }
 
-  // send to can
-  auto words = capnp::messageToFlatArray(msg);
-  auto bytes = words.asBytes();
-  zmq_send(s, bytes.begin(), bytes.size(), 0);
+  return frame_sent;
 }
 
 void can_health(void *s) {
@@ -456,11 +476,53 @@ 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, cur_time, last_long_sleep;
+  int recv_state = 0;
+  force_send = true;
+  last_long_sleep = 1e-3 * nanos_since_boot();
+  wake_time = last_long_sleep;
+
   while (!do_exit) {
-    can_recv(publisher);
-    // 5ms
-    usleep(5*1000);
+
+    frame_sent = can_recv(publisher, 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;
+      cur_time = 1e-3 * nanos_since_boot();
+      if (wake_time > cur_time) {
+        usleep(wake_time - cur_time);
+      }
+    }
   }
   return NULL;
 }
@@ -644,7 +706,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

@@ -334,15 +334,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));
@@ -354,10 +358,9 @@ class CANParser {
       auto cans = event.getCan();
 
       UpdateCans(sec, cans);
+      UpdateValid(sec);
     }
 
-    UpdateValid(sec);
-
     zmq_msg_close(&msg);
   }
 
@@ -386,6 +389,7 @@ class CANParser {
 
  private:
   const int bus;
+  bool drain = true;
   // zmq vars
   void *context = NULL;
   void *subscriber = NULL;

selfdrive/car/chrysler/interface.py

@@ -78,6 +78,8 @@ def get_params(candidate, fingerprint):
     ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
+    ret.steerMPCReactTime = 0.025     # increase total MPC projected time by 25 ms
+    ret.steerMPCDampTime = 0.25       # dampen desired angle over 250ms (5 mpc cycles)
 
     if candidate == CAR.JEEP_CHEROKEE:
       ret.wheelbase = 2.91  # in meters
@@ -138,7 +140,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)