Revert "Merge with kegman-gernbyFFsteer-latest (commaai#69)"

This reverts commit 99031cf.
Gernby · Dec 16, 2018 · 0171235 · 0171235
1 parent 99031cf
commit 0171235
Show file tree

Hide file tree

Showing 7 changed files with 43 additions and 233 deletions.
diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
@@ -166,9 +166,6 @@ 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) {
@@ -247,17 +244,8 @@ 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());
@@ -266,38 +254,27 @@ 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) {
@@ -340,8 +317,6 @@ class CANParser {
       UpdateCans(sec, cans);
     }
 
-    if (can_forward_period_ns > 0) ForwardCANData(sec);
-
     UpdateValid(sec);
 
     zmq_msg_close(&msg);
@@ -376,11 +351,6 @@ 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;
 };

diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
@@ -36,7 +36,6 @@ 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),
@@ -243,11 +242,7 @@ 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']
-    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 = 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']

diff --git a/selfdrive/car/honda/interface.py b/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.6], [0.18]]
+      ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.8], [0.24]]
       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.6], [0.18]]
+      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
       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.6], [0.18]]
+      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
       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.6], [0.18]]
+      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]

diff --git a/selfdrive/controls/controlsd.py b/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, CS.steeringRate, 
+  actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
                                                      CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)
 
   # Send a "steering required alert" if saturation count has reached the limit