Revert "Gernby/Feed-Forward-Accel-Rate refactored for tesla"

This reverts commit 06c88c9.
jeankalud · Dec 7, 2018 · b3127be · b3127be
1 parent 06c88c9
commit b3127be
Show file tree

Hide file tree

Showing 7 changed files with 46 additions and 171 deletions.
diff --git a/launch_chffrplus.sh b/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

diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
@@ -153,7 +153,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   int addr = to_fwd->RIR>>21;
   if (bus_num == 0) {
     return 2;
-  } else if (bus_num == 2 && addr != 0xE4 && addr != 0xE5 && addr != 0x194 && addr != 0x1FA &&
+  } else if (bus_num == 2 && addr != 0xE4 && addr != 0x194 && addr != 0x1FA &&
              addr != 0x30C && addr != 0x33D && addr != 0x39F) {
     return 0;
   }
@@ -164,7 +164,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   if (bus_num == 1 || bus_num == 2) {
     int addr = to_fwd->RIR>>21;
-    return addr != 0xE4 && addr != 0xE5 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1;
+    return addr != 0xE4 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1;
   }
   return -1;
 }

diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
@@ -171,9 +171,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) {
@@ -252,17 +249,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());
@@ -271,38 +259,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) {
@@ -345,8 +322,6 @@ class CANParser {
       UpdateCans(sec, cans);
     }
 
-    if (can_forward_period_ns > 0) ForwardCANData(sec);
-
     UpdateValid(sec);
 
     zmq_msg_close(&msg);
@@ -381,11 +356,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),
@@ -237,11 +236,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
@@ -370,7 +370,7 @@ def get_params(candidate, fingerprint):
     ret.startAccel = 0.5
 
     ret.steerActuatorDelay = 0.1
-    ret.steerRateCost = 0.35
+    ret.steerRateCost = 0.5
 
     return ret
 

diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
@@ -179,7 +179,7 @@ def get_params(candidate, fingerprint):
     ret.stoppingControl = True
     ret.steerLimitAlert = False
     ret.startAccel = 0.5
-    ret.steerRateCost = 0.5.
+    ret.steerRateCost = 1.
 
     return ret
 

diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -1,8 +1,5 @@
-import zmq
 import math
 import numpy as np
-import time
-import json
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
@@ -40,25 +37,6 @@ def __init__(self, CP):
                             k_f=CP.steerKf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
     self.setup_mpc(CP.steerRateCost)
-    self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT      # Multiplier for inductive component (feed forward)
-    self.projection_factor = 5.0 * _DT                       #  Mutiplier for reactive component (PI)
-    self.accel_limit = 5.0                                 # Desired acceleration limit to prevent "whip steer" (resistive component)
-    self.ff_angle_factor = 0.5         # Kf multiplier for angle-based feed forward
-    self.ff_rate_factor = 5.0         # Kf multiplier for rate-based feed forward
-    self.ratioDelayExp = 2.0           # Exponential coefficient for variable steering rate (delay)
-    self.ratioDelayScale = 0.0          # Multiplier for variable steering rate (delay)
-    self.prev_angle_rate = 0
-    self.feed_forward = 0.0
-    self.angle_rate_desired = 0.0
-
-    # variables for dashboarding
-    self.context = zmq.Context()
-    self.steerpub = self.context.socket(zmq.PUB)
-    self.steerpub.bind("tcp://*:8594")
-    self.steerdata = ""
-    self.frames = 0
-    self.curvature_factor = 0.0
-    self.slip_factor = 0.0
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -76,64 +54,45 @@ def setup_mpc(self, steer_rate_cost):
     self.last_mpc_ts = 0.0
     self.angle_steers_des = 0.0
     self.angle_steers_des_mpc = 0.0
-    self.angle_steers_des_prev = 0.0
-    self.angle_steers_des_time = 0.0
+    self.starting_angle_steers = 0.0
+    self.avg_angle_rate = 0.0
+    self.angle_rate_count = 0.0
 
   def reset(self):
     self.pid.reset()
 
   def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL):
-    cur_time = sec_since_boot()    
     self.mpc_updated = False
     # TODO: this creates issues in replay when rewinding time: mpc won't run
     if self.last_mpc_ts < PL.last_md_ts:
       self.last_mpc_ts = PL.last_md_ts
-      self.angle_steers_des_prev = self.angle_steers_des_mpc
-
-      self.curvature_factor = VM.curvature_factor(v_ego)
-
-      # This is currently disabled, but it is used to compensate for variable steering rate
-      ratioDelayFactor = 1. + self.ratioDelayScale * abs(angle_steers / 100.) ** self.ratioDelayExp
-
-      # Determine a proper delay time that includes the model's processing time, which is variable
-      plan_age = _DT_MPC + cur_time - float(PL.last_md_ts / 1000000000.0) 
-      total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age
-
-      # Use steering rate from the last 2 samples to estimate acceleration for a more realistic future steering rate
-      accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate    
+      self.starting_angle_steers = angle_steers
+      self.avg_angle_rate = 0.0
+      self.angle_rate_count = 0.0
 
-      # Project the future steering angle for the actuator delay only (not model delay)
-      projected_angle_steers = ratioDelayFactor * CP.steerActuatorDelay * accelerated_angle_rate + angle_steers
+      curvature_factor = VM.curvature_factor(v_ego)
 
-      self.l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
-      self.r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
-      self.p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
+      l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
+      r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
+      p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
 
-      # account for actuation delay and the age of the plan
-      self.cur_state = calc_states_after_delay(self.cur_state, v_ego, projected_angle_steers, self.curvature_factor, CP.steerRatio, total_delay)
+      # account for actuation delay
+      self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
 
       v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
       self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                          self.l_poly, self.r_poly, self.p_poly,
-                          PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, self.curvature_factor, v_ego_mpc, PL.PP.lane_width)
+                          l_poly, r_poly, p_poly,
+                          PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
 
       # reset to current steer angle if not active or overriding
       if active:
-        self.isActive = 1
         delta_desired = self.mpc_solution[0].delta[1]
       else:
-        self.isActive = 0
         delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
 
       self.cur_state[0].delta = delta_desired
 
       self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
-
-      # Use the model's solve time instead of cur_time
-      self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0)
-
-      # Use last 2 desired angles to determine the model's desired steer rate
-      self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC
       self.mpc_updated = True
 
       #  Check for infeasable MPC solution
@@ -147,81 +106,32 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
           self.last_cloudlog_t = t
           cloudlog.warning("Lateral mpc - nan: True")
 
-    elif self.steerdata != "" and self.frames > 10:
-      self.steerpub.send(self.steerdata)
-      self.frames = 0
-      self.steerdata = ""
-
     if v_ego < 0.3 or not active:
       output_steer = 0.0
-      self.feed_forward = 0.0
       self.pid.reset()
     else:
-      # Interpolate desired angle between MPC updates
-      self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time)
+      # Calculate average steering rate since last MPC update
+      self.avg_angle_rate = (self.avg_angle_rate * self.angle_rate_count + angle_rate) / (self.angle_rate_count + 1.) 
+      self.angle_rate_count += 1.0
 
-      # Determine the target steer rate for desired angle, but prevent the acceleration limit from being exceeded
-      # Restricting the steer rate creates the resistive component needed for resonance
-      restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit)
-
-      # Determine projected desired angle that is within the acceleration limit (prevent the steering wheel from jerking)
-      projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate
-
-      # Determine project angle steers using current steer rate
-      projected_angle_steers = float(angle_steers) + self.projection_factor * float(angle_rate)
+      if abs(angle_rate) <= 1. or abs(angle_steers - self.angle_steers_des_mpc) <= 0.2:
+        # For small steering error, use instantaneous angle for torque calculation
+        future_angle_steers = angle_steers
+      else:
+        # Otherwise, use average steering rate since last MPC update to project future steering error
+        future_angle_steers = (self.avg_angle_rate * _DT_MPC) + self.starting_angle_steers
 
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
-
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
-        # Decide which feed forward mode should be used (angle or rate).  Use more dominant mode, and only if conditions are met
-        # Spread feed forward out over a period of time to make it more inductive (for resonance)
-        if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \
-            and (abs(float(restricted_steer_rate)) > abs(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \
-            and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0):
-          ff_type = "r"
-          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / self.smooth_factor  
-        elif abs(self.angle_steers_des - float(angle_offset)) > 0.5:
-          ff_type = "a"
-          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor  
-        else:
-          ff_type = "r"
-          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor  
+        steer_feedforward = apply_deadzone(self.angle_steers_des_mpc - float(angle_offset), 0.5) 
+        steer_feedforward *= v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
       else:
-        self.feed_forward = self.angle_steers_des_mpc   # feedforward desired angle
+        steer_feedforward = self.angle_steers_des_mpc   # feedforward desired angle
       deadzone = 0.0
 
-      # Use projected desired and actual angles instead of "current" values, in order to make PI more reactive (for resonance)
-      output_steer = self.pid.update(projected_angle_steers_des, projected_angle_steers, check_saturation=(v_ego > 10), override=steer_override,
-                                     feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)
-
-
-      # All but the last 3 lines after here are for real-time dashboarding
-      self.pCost = 0.0
-      self.lCost = 0.0
-      self.rCost = 0.0
-      self.hCost = 0.0
-      self.srCost = 0.0
-      self.last_ff_a = 0.0
-      self.last_ff_r = 0.0
-      self.center_angle = 0.0
-      self.steer_zero_crossing = 0.0
-      self.steer_rate_cost = 0.0
-      self.avg_angle_rate = 0.0
-      self.angle_rate_count = 0.0
-      driver_torque = 0.0     
-      steer_motor = 0.0
-      self.frames += 1
-
-      self.steerdata += ("%d,%s,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d|" % (self.isActive, \
-      ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, self.cur_state[0].psi, self.cur_state[0].delta, self.cur_state[0].t, self.curvature_factor, self.slip_factor ,self.smooth_factor, self.accel_limit, float(restricted_steer_rate) ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, steer_motor, float(driver_torque), \
-      self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, projected_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \
-      self.angle_steers_des_mpc, CP.steerRatio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \
-      PL.PP.r_prob, PL.PP.c_prob, PL.PP.p_prob, self.l_poly[0], self.l_poly[1], self.l_poly[2], self.l_poly[3], self.r_poly[0], self.r_poly[1], self.r_poly[2], self.r_poly[3], \
-      self.p_poly[0], self.p_poly[1], self.p_poly[2], self.p_poly[3], PL.PP.c_poly[0], PL.PP.c_poly[1], PL.PP.c_poly[2], PL.PP.c_poly[3], PL.PP.d_poly[0], PL.PP.d_poly[1], \
-      PL.PP.d_poly[2], PL.PP.lane_width, PL.PP.lane_width_estimate, PL.PP.lane_width_certainty, v_ego, self.pid.p, self.pid.i, self.pid.f, int(time.time() * 100) * 10000000))
-
+      output_steer = self.pid.update(self.angle_steers_des_mpc, future_angle_steers, check_saturation=(v_ego > 10), override=steer_override,
+                                     feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
     self.sat_flag = self.pid.saturated
-    self.prev_angle_rate = angle_rate
-    return output_steer, float(self.angle_steers_des)
+    return output_steer, float(self.angle_steers_des_mpc)