Gernby's FF steering PR candidate (#97)

* Fixed feed-forward to consider steer_offset

* fixed a small oops on types

* Testing strategies for zero-crossing

* Moved angle stat collection to latcontrol.py

* First version that runs

* added outbound interface for feed-forward table

* First working

* Added more metrics

* Adjusted parameter timing

* performance tweaks

* Cleanup

* untested tweaks

* minor tweaks

* many untested changes

* going the other way with some things...

* Best yet

* cleaned up personalized "stuff"

* more cleanup for general use

* untested: minor adjustment to further reduce noise

* Fixed defect in desired angle interpolation

* some cleanup

* reverted change to Ki

* Reverted changes to manager.py

* Added steering rate consideration to latcontrol

* cleaned up for PR

* Fixed merge

* Testing approach when desired angle close to actual

* trying rate-based feed-forward

* added self-tuning parms for rate and angle FF

* fixed trim logic, and persisted to flash

* working amazingly well

* decreased time lapse for angle-based FF adjust

* many tweaks, self-tuning is a bitch

* simulteneous dual mode feedforward working very well

* added angular accelleration limit

* added super awesome angular accel limit

* non-specific save

* switching directions again

* oops

* ugly code with amazing results

* awesome, but untested after some cleanup

* more cleanup

* cleanup of the cleanup?  Need to test

* works amazingly well ... big O face

* cleanup

* I wish git was batter for cleanup

* hopefully proper merge of merged fixes of merge

* fixed an oops for non-bosch hondas

* added steer rate to future state calculation

* added actual acceleration to future projected state

* fixed projected angle error for PIF

* untested ...

* added comments

* completely UNtested

* untested merge

* battling shitty git

* git is sucking giant monkey balls

* Trying to figure out what git did...

* still trying to un-git myself

* Finally got it fixed, works awesome!

* Hopefully not corrupted by git

* hopefully fixed white space corruption

* working again, hopefully resolved git issue

* removed hard-coded actuator delay

* Fixed steer_actuator_delay for pathplanner

* Wiggle vision + resonant FF and error cancellation

* increased projection factor and decreased accel_limit

* enhanced torque logic to use accelerated rate

* added smoothing of mpc delta values

* trying different interpolation logic

* added update to cur_state.delta

* very smooth interpolation between delayed MPC updates

* increased Kf for Pilot

* much better interpolation plus Civic tweaks

* tiny meaningless changes

* Fixed honda interface values

* removed steer_offset from pilot

* Tweaked steer_offset list

* Reverted a couple resonant parameters

Reduced projection_factor and increased accel_limit

* Enabled error correction

* changed Kp for some

* Works great... time to clean it up

* reverted accelerated_angle_rate for pid updates

* reverted visiond

* Increased error correction

* Reverted projection_factor

* fixed projected_angle_steers for future statefixed projected_angle_steers for future state

* Added simplified tuning parameters

* last commit didn't have saved values

* not working ... lane filtering

* added calculated steer rate

* updated toyota carstate

* add dual-mode steer-rate alternating

* reverted toyota carstate change

* enabled auto-selection of steer_rate

* removed extra variable

* untested: cleaning up for PR

* Fixed a cleanup issue

* added comments in carstate

* more cleanup

* not quite ready

* added vibration

* need to test, but I think it's ready

* Update carstate.py

Author: NeonGalaxy75

cereal/car.capnp

@@ -327,6 +327,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
@@ -341,7 +342,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);
   longitudinalKpV @37 :List(Float32);

selfdrive/car/honda/carcontroller.py

@@ -147,7 +147,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/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 = 0.5
+      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,9 @@ 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']
+
+    # The defined value in the DBC is always positive, so the calculated value needs to be used
+    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/toyota/carstate.py

@@ -134,7 +134,13 @@ 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']
+
+    # Only use the reported steer rate from some Toyotas, since others are very noisy
+    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

selfdrive/controls/lib/latcontrol.py

@@ -12,8 +12,8 @@
 _DT_MPC = 0.05  # 20Hz
 
 
-def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
-  states[0].x = v_ego * delay
+def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay, long_camera_offset):
+  states[0].x = max(0.0, v_ego * delay + long_camera_offset)
   states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
   return states
 
@@ -22,99 +22,188 @@ def get_steer_max(CP, v_ego):
   return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
 
 
+def apply_deadzone(angle, deadzone):
+  if angle > deadzone:
+    angle -= deadzone
+  elif angle < -deadzone:
+    angle += deadzone
+  else:
+    angle = 0.
+  return angle
+
+
 class LatControl(object):
   def __init__(self, CP):
-    self.pid = PIController((CP.steerKpBP, CP.steerKpV),
-                            (CP.steerKiBP, CP.steerKiV),
-                            k_f=CP.steerKf, pos_limit=1.0)
+
+    # Eliminate break-points, since they aren't needed (and would cause problems for resonance)
+    KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV) * CP.steerReactance]
+    KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * CP.steerReactance]
+    Kf = CP.steerKf * CP.steerInductance
+    self.pid = PIController(([0.], KpV),
+                            ([0.], KiV),
+                            k_f=Kf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
     self.setup_mpc(CP.steerRateCost)
+    self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT    # Multiplier for inductive component (feed forward)
+    self.projection_factor = CP.steerReactance * 5.0 * _DT                         # Mutiplier for reactive component (PI)
+    self.accel_limit = 2.0 / CP.steerResistance                                    # 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.prev_angle_rate = 0
+    self.feed_forward = 0.0
+    self.last_mpc_ts = 0.0
+    self.angle_steers_des = 0.0
+    self.angle_steers_des_time = 0.0
+    self.angle_steers_des_mpc = 0.0
+    self.projected_angle_steers = 0.0
+    self.steer_counter = 1.0
+    self.steer_counter_prev = 0.0
+    self.rough_steers_rate = 0.0
+    self.prev_angle_steers = 0.0
+    self.calculate_rate = True
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
     self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
 
     self.mpc_solution = libmpc_py.ffi.new("log_t *")
     self.cur_state = libmpc_py.ffi.new("state_t *")
+    self.mpc_angles = [0.0, 0.0, 0.0]
+    self.mpc_times = [0.0, 0.0, 0.0]
     self.mpc_updated = False
     self.mpc_nans = False
     self.cur_state[0].x = 0.0
     self.cur_state[0].y = 0.0
     self.cur_state[0].psi = 0.0
     self.cur_state[0].delta = 0.0
 
-    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
-
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL):
-    cur_time = sec_since_boot()
+  def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL):
     self.mpc_updated = False
+
+    if angle_rate == 0.0 and self.calculate_rate:
+      if angle_steers != self.prev_angle_steers:
+        self.steer_counter_prev = self.steer_counter
+        self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0
+        self.steer_counter = 0.0
+      elif self.steer_counter >= self.steer_counter_prev:
+        self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0)
+      self.steer_counter += 1.0
+      angle_rate = self.rough_steers_rate
+
+      # Don't use accelerated rate unless it's from CAN
+      accelerated_angle_rate = angle_rate
+    else:
+      # If non-zero angle_rate is provided, use it instead
+      self.calculate_rate = False
+      # Use steering rate from the last 2 samples to estimate acceleration for a likely future steering rate
+      accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate
+
     # 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
-
+      cur_time = sec_since_boot()
+      mpc_time = float(self.last_mpc_ts / 1000000000.0)
       curvature_factor = VM.curvature_factor(v_ego)
 
+      # Determine future angle steers using steer rate
+      projected_angle_steers = float(angle_steers) + CP.steerActuatorDelay * float(accelerated_angle_rate)
+
+      # Determine a proper delay time that includes the model's variable processing time
+      plan_age = _DT_MPC + cur_time - mpc_time
+      total_delay = CP.steerActuatorDelay + plan_age
+
       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
-      self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
+      # 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, curvature_factor,
+                                                      CP.steerRatio, total_delay, CP.eonToFront)
 
       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,
                           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:
-        delta_desired = self.mpc_solution[0].delta[1]
-      else:
-        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)
-      self.angle_steers_des_time = cur_time
       self.mpc_updated = True
 
       #  Check for infeasable MPC solution
       self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
-      t = sec_since_boot()
-      if self.mpc_nans:
+      if not self.mpc_nans:
+        self.mpc_angles = [self.angle_steers_des,
+                          float(math.degrees(self.mpc_solution[0].delta[1] * CP.steerRatio) + angle_offset),
+                          float(math.degrees(self.mpc_solution[0].delta[2] * CP.steerRatio) + angle_offset)]
+
+        self.mpc_times = [self.angle_steers_des_time,
+                          mpc_time + _DT_MPC,
+                          mpc_time + _DT_MPC + _DT_MPC]
+
+        self.angle_steers_des_mpc = self.mpc_angles[1]
+      else:
         self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
         self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
 
-        if t > self.last_cloudlog_t + 5.0:
-          self.last_cloudlog_t = t
+        if cur_time > self.last_cloudlog_t + 5.0:
+          self.last_cloudlog_t = cur_time
           cloudlog.warning("Lateral mpc - nan: True")
 
+    cur_time = sec_since_boot()
+    self.angle_steers_des_time = cur_time
+
     if v_ego < 0.3 or not active:
       output_steer = 0.0
+      self.feed_forward = 0.0
       self.pid.reset()
+      self.angle_steers_des = angle_steers
+      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / CP.steerRatio
     else:
-      # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
-      # constant for 0.05s.
-      #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT  # no greater than dt mpc + dt, to prevent too high extraps
-      #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
-      self.angle_steers_des = self.angle_steers_des_mpc
+      # Interpolate desired angle between MPC updates
+      self.angle_steers_des = np.interp(cur_time, self.mpc_times, self.mpc_angles)
+      self.angle_steers_des_time = cur_time
+      self.cur_state[0].delta = math.radians(self.angle_steers_des - angle_offset) / CP.steerRatio
+
+      # 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(accelerated_angle_rate) - self.accel_limit,
+                                                                                    float(accelerated_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 future angle steers using accellerated steer rate
+      self.projected_angle_steers = float(angle_steers) + self.projection_factor * float(accelerated_angle_rate)
+
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
-      steer_feedforward = self.angle_steers_des   # feedforward desired angle
-      if CP.steerControlType == car.CarParams.SteerControlType.torque:
-        steer_feedforward *= v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
       deadzone = 0.0
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
-                                     feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
+
+      if CP.steerControlType == car.CarParams.SteerControlType.torque:
+        # Decide which feed forward mode should be used (angle or rate).  Use more dominant mode, but only if conditions are met
+        # Spread feed forward out over a period of time to make it 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(accelerated_angle_rate) or (float(restricted_steer_rate) < 0) != (accelerated_angle_rate < 0)) \
+            and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0):
+          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:
+          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:
+          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor
+
+        # 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, self.projected_angle_steers, check_saturation=(v_ego > 10),
+                                        override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)
 
     self.sat_flag = self.pid.saturated
-    return output_steer, float(self.angle_steers_des)
+    self.prev_angle_rate = angle_rate
+    self.prev_angle_steers = angle_steers
+
+    # return MPC angle in the unused output (for ALCA)
+    if CP.steerControlType == car.CarParams.SteerControlType.torque:
+      return output_steer, self.mpc_angles[1]
+    else:
+      return self.mpc_angles[1], float(self.angle_steers_des)