much improved auto-tune

dashboard.py

@@ -140,8 +140,8 @@ def dashboard_thread(rate=300):
               last_actual = l100.live100.angleSteers
               v_curv = l100.live100.curvature
 
-              influxLineString += ("opData,sources=capnp actualNoise=%1.3f,desiredNoise=%1.3f,ff_rate=%1.3f,ff_angle=%1.3f,ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" %
-                          (l100.live100.actualNoise, l100.live100.desiredNoise, l100.live100.rateModeFF, l100.live100.angleModeFF, angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid,
+              influxLineString += ("opData,sources=capnp angleGain=%1.2f,rateGain=%1.4f,actualNoise=%1.3f,desiredNoise=%1.3f,ff_rate=%1.3f,ff_angle=%1.3f,ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" %
+                          (l100.live100.angleGain, l100.live100.rateGain, l100.live100.actualNoise, l100.live100.desiredNoise, l100.live100.rateModeFF, l100.live100.angleModeFF, angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid,
                           l100.live100.upSteer, l100.live100.uiSteer, l100.live100.ufSteer, l100.live100.cumLagMs, l100.live100.vCruise, receiveTime))
               frame_count += 1
 

selfdrive/car/honda/carcontroller.py

@@ -165,7 +165,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       # If using stock ACC, spam cancel command to kill gas when OP disengages.
       if pcm_cancel_cmd:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx))
-      elif CS.stopped:
+      elif CS.stopped and CS.lead_distance is not None and CS.lead_distance > 45:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx))
 
     else:

selfdrive/controls/lib/latcontrol.py

@@ -35,36 +35,24 @@ def __init__(self, CP):
                             k_f=CP.steerKf, pos_limit=1.0)
 
   def adjust_angle_gain(self):
-    if self.angle_ff_counter > 100:
-      if abs(self.dampened_desired_angle) > abs(self.last_angle):
-        if self.pid.i > self.starting_integral:
-          if self.pid.f > 0:
-            self.angle_ff_gain *= 1.01
-          else:
-            self.angle_ff_gain *= 0.99
-        elif self.pid.i < self.starting_integral:
-          if self.pid.f < 0:
-            self.angle_ff_gain *= 1.01
-          else:
-            self.angle_ff_gain *= 0.99
-      self.angle_ff_counter = 0
-      self.last_angle = self.dampened_desired_angle
-      self.starting_integral = self.pid.i
-    elif self.angle_ff_counter == 0:
-      self.starting_integral = self.pid.i
-      self.last_angle = self.dampened_desired_angle
-    self.angle_ff_counter += 1
+    if self.pid.i > self.previous_integral:
+      if self.pid.f > 0 and self.pid.i > 0:
+        self.angle_ff_gain *= 1.0001
+      else:
+        self.angle_ff_gain *= 0.9999
+    elif self.pid.i < self.previous_integral:
+      if self.pid.f < 0 and self.pid.i < 0:
+        self.angle_ff_gain *= 1.0001
+      else:
+        self.angle_ff_gain *= 0.9999
+    self.previous_integral = self.pid.i
 
   def adjust_rate_gain(self, steer_rate):
-    self.desired_rate_noise += 0.0001 * (self.dampened_desired_rate**2 - self.desired_rate_noise)
     self.actual_rate_noise += 0.0001 * (steer_rate**2 - self.actual_rate_noise)
-    #if self.rate_ff_counter > 100:
-    if self.actual_rate_noise**0.5 > 0.75 * self.desired_rate_noise**0.5:
+    if self.actual_rate_noise**0.5 > 0.5:
       self.rate_ff_gain *= 0.9999
     else:
       self.rate_ff_gain *= 1.0001
-    #  self.rate_ff_counter = 0
-    #self.rate_ff_counter += 1
 
   def live_tune(self, CP):
     self.mpc_frame += 1
@@ -105,7 +93,7 @@ def update(self, active, v_ego, angle_steers, rate_steers, steer_override, CP, V
       self.angle_ff_counter = 0
       self.rate_ff_counter = 0
       self.desired_rate_noise = 0.0
-      self.starting_integral = 0.0
+      self.previous_integral = 0.0
       self.actual_rate_noise = 0.0
       self.last_angle = 0.0
     else: