new conflict?

cereal/car.capnp

@@ -323,6 +323,7 @@ struct CarParams {
   centerToFront @9 :Float32;   # [m] GC distance to front axle
   steerRatio @10 :Float32;       # [] ratio between front wheels and steering wheel angles
   steerRatioRear @11 :Float32;  # [] rear steering ratio wrt front steering (usually 0)
+  eonToFront  @54  :Float32;    # [m] distance from EON to front wheels
 
   # things we can derive
   rotationalInertia @12 :Float32;    # [kg*m2] body rotational inertia
@@ -337,6 +338,10 @@ struct CarParams {
   steerKpDEPRECATED @15 :Float32;
   steerKiDEPRECATED @16 :Float32;
   steerKf @25 :Float32;
+  steerReactance @51 :Float32;
+  steerInductance @52 :Float32;
+  steerResistance @53 :Float32;
+
 
   # Kp and Ki for the longitudinal control
   longitudinalKpBP @36 :List(Float32);

launch_chffrplus.sh

@@ -6,11 +6,11 @@ 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
+  #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

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/gm/interface.py

@@ -66,6 +66,11 @@ def get_params(candidate, fingerprint):
 
     std_cargo = 136
 
+    ret.steerReactance = 1.0
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
+
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
       ret.minEnableSpeed = 18 * CV.MPH_TO_MS

selfdrive/car/honda/carcontroller.py

@@ -133,7 +133,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
 
     # *** compute control surfaces ***
     BRAKE_MAX = 1024/4
-    if CS.CP.carFingerprint in (CAR.ACURA_ILX):
+    if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.ACURA_ILX):
       STEER_MAX = 0xF00
     elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
       STEER_MAX = 0x3e8  # CR-V only uses 12-bits and requires a lower value (max value from energee)

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),
@@ -239,7 +240,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  (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH):
+      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

@@ -186,6 +186,10 @@ def get_params(candidate, fingerprint):
     ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
 
     ret.steerKf = 0.00006 # conservative feed-forward
+    ret.steerReactance = 1.0
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     if candidate == CAR.CIVIC:
       stop_and_go = True
@@ -226,7 +230,12 @@ 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.steerKpV, ret.steerKiV = [[0.6], [0.18]]
+      ret.steerReactance = 1.0
+      ret.steerInductance = 1.0
+      ret.steerResistance = 1.0
+      ret.eonToFront = 1.0
+      ret.steerKpV, ret.steerKiV = [[0.64], [0.192]]
+      ret.steerKf = 0.000064
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]

selfdrive/car/hyundai/carstate.py

@@ -184,7 +184,7 @@ def update(self, cp, cp_cam):
     self.standstill = not self.v_wheel > 0.1
 
     self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
-    self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
+    self.angle_steers_rate = 0.0  #cp.vl["SAS11"]['SAS_Speed']
     self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
     self.main_on = True
     self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']

selfdrive/car/hyundai/interface.py

@@ -72,6 +72,11 @@ def get_params(candidate, fingerprint):
     ret.steerActuatorDelay = 0.1  # Default delay
     tire_stiffness_factor = 1.
 
+    ret.steerReactance = 1.0
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
+
     if candidate == CAR.SANTA_FE:
       ret.steerKf = 0.00005
       ret.steerRateCost = 0.5

selfdrive/car/mock/interface.py

@@ -49,6 +49,11 @@ def get_params(candidate, fingerprint):
     ret.safetyModel = car.CarParams.SafetyModels.noOutput
     ret.openpilotLongitudinalControl = False
 
+    ret.steerReactance = 1.0
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
+
     # FIXME: hardcoding honda civic 2016 touring params so they can be used to
     # scale unknown params for other cars
     ret.mass = 1700.

selfdrive/car/toyota/carstate.py

@@ -134,7 +134,12 @@ def update(self, cp, cp_cam):
     self.standstill = not self.v_wheel > 0.001
 
     self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-    self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
+
+    if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.COROLLA):
+      self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
+    else:
+      self.angle_steers_rate = 0.0
+
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
     self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
     self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']

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