Merge branch 'kegman-gernbyFFsteer-latest' into kegman-devel

selfdrive/can/parser.cc

@@ -166,6 +166,9 @@ class CANParser {
     subscriber = zmq_socket(context, ZMQ_SUB);
     zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
 
+    forwarder = zmq_socket(context, ZMQ_PUB);
+    zmq_bind(forwarder, "tcp://*:8592");
+
     std::string tcp_addr_str;
 
     if (sendcan) {
@@ -244,8 +247,17 @@ class CANParser {
       // parse the messages
       for (int i = 0; i < msg_count; i++) {
         auto cmsg = cans[i];
+
+        if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
+        uint8_t dat[8] = {0};
+        memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
+
+        if (can_forward_period_ns > 0) raw_can_values[cmsg.getSrc()][cmsg.getAddress()] = read_u64_be(dat);
+
+        //if (((cmsg.getAddress() == 0xe4 or cmsg.getAddress() == 0x33d) and cmsg.getSrc() == bus) or \
+        //     (cmsg.getSrc() != bus and cmsg.getAddress() != 0x33d and cmsg.getAddress() != 0xe4 and \
+        //     (cmsg.getAddress() < 0x240 or cmsg.getAddress() > 0x24A))) {
         if (cmsg.getSrc() != bus) {
-          // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
           continue;
         }
         auto state_it = message_states.find(cmsg.getAddress());
@@ -254,27 +266,38 @@ class CANParser {
           continue;
         }
 
-        if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
-        uint8_t dat[8] = {0};
-        memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
-
         // Assumes all signals in the message are of the same type (little or big endian)
         // TODO: allow signals within the same message to have different endianess
         auto& sig = message_states[cmsg.getAddress()].parse_sigs[0];
         if (sig.is_little_endian) {
             p = read_u64_le(dat);
         } else {
             p = read_u64_be(dat);
-        }
+        } 
 
         DEBUG("  proc %X: %llx\n", cmsg.getAddress(), p);
 
         state_it->second.parse(sec, cmsg.getBusTime(), p);
       }
   }
 
+  void ForwardCANData(uint64_t sec) {
+      if (sec > next_can_forward_ns) {
+          next_can_forward_ns += can_forward_period_ns;
+          // next_can_forward_ns starts at 0, so it needs to be reset.  Also handle delays.
+          if (sec > next_can_forward_ns) next_can_forward_ns = sec + can_forward_period_ns;
+          std::string canOut = "";
+          for (auto src : raw_can_values) {
+              for (auto pid : src.second) {
+                  canOut = canOut + std::to_string(src.first) + " " + std::to_string(pid.first) + " " + std::to_string(pid.second) + "|";
+              }
+          }
+          zmq_send(forwarder, canOut.data(), canOut.size(), 0);
+      }
+  }
+
   void UpdateValid(uint64_t sec) {
-    can_valid = true;
+    can_valid = true; 
     for (const auto& kv : message_states) {
       const auto& state = kv.second;
       if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
@@ -317,6 +340,8 @@ class CANParser {
       UpdateCans(sec, cans);
     }
 
+    if (can_forward_period_ns > 0) ForwardCANData(sec);
+
     UpdateValid(sec);
 
     zmq_msg_close(&msg);
@@ -351,6 +376,11 @@ class CANParser {
   void *context = NULL;
   void *subscriber = NULL;
 
+  void *forwarder = NULL;
+  uint64_t can_forward_period_ns = 100000000;
+  uint64_t next_can_forward_ns = 0;
+  std::unordered_map<uint32_t, std::unordered_map<uint32_t, uint64_t>> raw_can_values;
+
   const DBC *dbc = NULL;
   std::unordered_map<uint32_t, MessageState> message_states;
 };

selfdrive/car/honda/carstate.py

@@ -36,6 +36,7 @@ def get_can_signals(CP):
       ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
       ("STEER_ANGLE", "STEERING_SENSORS", 0),
       ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
+      ("STEER_ANGLE_OFFSET", "STEERING_SENSORS", 0),
       ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
       ("LEFT_BLINKER", "SCM_FEEDBACK", 0),
       ("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
@@ -242,7 +243,11 @@ def update(self, cp, cp_cam):
       self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
 
     self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
-    self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
+    if self.CP.carFingerprint in HONDA_BOSCH:
+      self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET']
+    else:
+      self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
+
     self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
 
     #self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']

selfdrive/car/honda/interface.py

@@ -196,7 +196,7 @@ def get_params(candidate, fingerprint):
       tire_stiffness_factor = 1.
       # 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.33], [0.10]] if is_fw_modified else [[0.8], [0.24]]
+      ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.6], [0.18]]
       if is_fw_modified:
         ret.steerKf = 0.00003
       ret.longitudinalKpBP = [0., 5., 35.]
@@ -211,7 +211,7 @@ def get_params(candidate, fingerprint):
       ret.centerToFront = centerToFront_civic
       ret.steerRatio = 14.63  # 10.93 is spec end-to-end
       tire_stiffness_factor = 1.
-      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]
@@ -256,7 +256,7 @@ def get_params(candidate, fingerprint):
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.3         # as spec
       tire_stiffness_factor = 0.444 # not optimized yet
-      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]
@@ -283,7 +283,7 @@ def get_params(candidate, fingerprint):
       ret.centerToFront = ret.wheelbase * 0.38
       ret.steerRatio = 15.0         # as spec
       tire_stiffness_factor = 0.444 # not optimized yet
-      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]

selfdrive/controls/controlsd.py

@@ -274,7 +274,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
                                               v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget,
                                               CP, PL.lead_1)
   # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
+  actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, 
                                                      CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)
 
   # Send a "steering required alert" if saturation count has reached the limit