Merge Gernby's Resonant Steering (commaai#51)

* Learn_angle_steer min max increased to +/-7 degs

* Increase min one bar distance to 12m for autodist

* Merge Gernby's Feed-Forward-Accel-Rate Branch (commaai#49)

* 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

* removed Steer Offset because Pilot no likey

* Increase tailgating 1 bar resume to 0.5 m/s

* Gernby's resonant steering v2

* fixed an oops for non-bosch hondas

* CAMERA_OFFSET = 0.08

* More conservative tailgating distance and resume

* More conservative auto-distance for 1 bar setting

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 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/controls/controlsd.py

@@ -276,7 +276,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/drive_helpers.py

@@ -58,8 +58,8 @@ def rate_limit(new_value, last_value, dw_step, up_step):
 def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
   # while being in the middle of the lane
-  min_offset = -5.  # deg
-  max_offset = 5.  # deg
+  min_offset = -7.  # deg
+  max_offset = 7.  # deg
   alpha = 1. / 36000.  # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
   min_learn_speed = 1.
 

selfdrive/controls/lib/latcontrol.py

@@ -1,5 +1,8 @@
+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
@@ -21,6 +24,14 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
 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):
@@ -29,6 +40,16 @@ def __init__(self, CP):
                             k_f=CP.steerKf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
     self.setup_mpc(CP.steerRateCost)
+    self.context = zmq.Context()
+    self.steerpub = self.context.socket(zmq.PUB)
+    self.steerpub.bind("tcp://*:8594")
+    self.steerdata = ""
+    self.smooth_factor = CP.steerActuatorDelay / _DT
+    self.feed_forward = 0.0
+    self.angle_rate_desired = 0.0
+    self.ff_angle_factor = 1.0
+    self.ff_rate_factor = self.ff_angle_factor * 3.0
+    self.accel_limit = 5.0
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -48,11 +69,21 @@ def setup_mpc(self, steer_rate_cost):
     self.angle_steers_des_mpc = 0.0
     self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
+    self.context = zmq.Context()
+    self.steerpub = self.context.socket(zmq.PUB)
+    self.steerpub.bind("tcp://*:8594")
+    self.steerdata = ""
+    self.smooth_factor = 11.0 * _DT_MPC / _DT
+    self.feed_forward = 0.0
+    self.angle_rate_desired = 0.0
+    self.ff_angle_factor = 0.25
+    self.ff_rate_factor = 2.5
+    self.accel_limit = 0.3
 
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL):
+  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
@@ -62,28 +93,31 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs
 
       curvature_factor = VM.curvature_factor(v_ego)
 
-      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))
+      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))
 
       # 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,
-                          l_poly, r_poly, p_poly,
+                          self.l_poly, self.r_poly, self.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)
-      self.angle_steers_des_time = cur_time
+      self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0)
+      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
@@ -97,24 +131,63 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs
           self.last_cloudlog_t = t
           cloudlog.warning("Lateral mpc - nan: True")
 
+    elif self.steerdata != "" and len(self.steerdata) > 40000:
+      self.steerpub.send(self.steerdata)
+      self.steerdata = ""
+
     if v_ego < 0.3 or not active:
       output_steer = 0.0
+      self.feed_forward = 0.0
       self.pid.reset()
     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
+      self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time)
+      restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit)
+      future_angle_steers_des = float(angle_steers) + self.smooth_factor * _DT * restricted_steer_rate
+      future_angle_steers = float(angle_steers) + self.smooth_factor * _DT * float(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)
+        if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(future_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(future_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(future_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(future_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  
+      else:
+        self.feed_forward = self.angle_steers_des_mpc   # feedforward desired angle
       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)
+                                     feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)
+
+      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.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,%d|" % (self.isActive, \
+      ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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, future_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))
 
     self.sat_flag = self.pid.saturated
     return output_steer, float(self.angle_steers_des)

selfdrive/controls/lib/pathplanner.py

@@ -1,7 +1,7 @@
 from common.numpy_fast import interp
 from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
 
-CAMERA_OFFSET = 0.06  # m from center car to camera
+CAMERA_OFFSET = 0.08  # m from center car to camera
 
 class PathPlanner(object):
   def __init__(self):

selfdrive/controls/lib/planner.py

@@ -230,13 +230,13 @@ def update(self, CS, lead, v_cruise_setpoint):
 
     # Defining some variables to make the logic more human readable for auto distance override below
     # Is the car tailgating the lead car?
-    if x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.25:
+    if x_lead < 15 and self.v_rel >= -1 and self.v_rel < 0.25:
       self.tailgating = 1
     else:
       self.tailgating = 0
 
     # Is the car running surface street speeds?
-    if CS.vEgo < 18.06:
+    if CS.vEgo < 19.44:
       self.street_speed = 1
     else:
       self.street_speed = 0
@@ -251,9 +251,9 @@ def update(self, CS, lead, v_cruise_setpoint):
     if CS.readdistancelines == 1:
       # If one bar distance, auto set to 2 bar distance under current conditions to prevent rear ending lead car
       if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
-        TR=1.8
+        TR=2.2
         if self.lastTR != 0:
-          self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          self.libmpc.init(MPC_COST_LONG.TTC, 0.075, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
           self.lastTR = 0
       else:
         TR=0.9 # 10m at 40km/hr
@@ -522,7 +522,7 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
     self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
     self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
 
-    plan_send.plan.events = events
+    plan_send.plan.events = eventssetting
     plan_send.plan.mdMonoTime = self.last_md_ts
     plan_send.plan.l20MonoTime = self.last_l20_ts