added rate-based feed forward

cereal/log.capnp

@@ -401,8 +401,8 @@ struct Live100Data {
   dampAngleSteers @52 :Float32;
   dampAngleSteersDes @53 :Float32;
   dampRateSteersDes @54 :Float32;
-  rateModeFF @56 :Int32;
-  angleModeFF @57 :Int32;
+  rateModeFF @56 :Float32;
+  angleModeFF @57 :Float32;
   curvature @37 :Float32;       # path curvature from vehicle model
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;

dashboard.py

@@ -36,12 +36,12 @@ def dashboard_thread(rate=300):
   context = zmq.Context()
   poller = zmq.Poller()
   ipaddress = "127.0.0.1"
-  carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller)
+  #carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller)
   #can = messaging.sub_sock(context, service_list['can'].port, addr=ipaddress, poller=poller)
   can = "disabled"
   vEgo = 0.0
-  #pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller)
-  pathPlan = None
+  pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller)
+  #pathPlan = None
   live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=False, poller=poller)
   #liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller)
   liveParameters = None
@@ -140,7 +140,7 @@ def dashboard_thread(rate=300):
               last_actual = l100.live100.angleSteers
               v_curv = l100.live100.curvature
 
-              influxLineString += ("opData,sources=capnp ff_rate=%d,ff_angle=%d,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,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%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" %
+              influxLineString += ("opData,sources=capnp 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,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%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.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.dampAngleRateDes, l100.live100.dampAngleSteers, 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
@@ -297,15 +297,16 @@ def dashboard_thread(rate=300):
             if sample_str != "":
                 sample_str += ","
             a = _pathPlan.pathPlan.mpcAngles
+            r = _pathPlan.pathPlan.mpcRates
             p = _pathPlan.pathPlan
 
             #sample_str = ("lane_width=%1.2f,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" %
             #            (_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3],
             #                  _pathPlan.pathPlan.cProb,  _pathPlan.pathPlan.lProb,  _pathPlan.pathPlan.rProb, a[1], a[2], a[3],
             #                  a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18]))
-            sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f" %
+            sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc0=%1.2f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpcR0=%1.2f,mpcR1=%1.2f,mpcR2=%1.2f,mpcR3=%1.2f,mpcR4=%1.2f,mpcR5=%1.2f,mpcR6=%1.2f" %
                         (p.laneWidth, p.lPoly[0], p.rPoly[0], p.cPoly[0], p.dPoly[0], p.lPoly[1], p.rPoly[1], p.cPoly[1], p.dPoly[1], p.lPoly[2], p.rPoly[2], p.cPoly[2], p.dPoly[2],p.lPoly[3], p.rPoly[3], p.cPoly[3], p.dPoly[3],
-                              p.cProb,  p.lProb,  p.rProb, a[1], a[2], a[3]))
+                              p.cProb,  p.lProb,  p.rProb, a[0], a[1], a[2], a[3], a[4], a[5], a[6], r[0], r[1], r[2], r[3], r[4], r[5], r[6]))
             influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime)
             sample_str = ""
             frame_count += 1

selfdrive/controls/controlsd.py

@@ -355,6 +355,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "ufAccelCmd": float(LoC.pid.f),
     "angleSteersDes": float(path_plan.pathPlan.angleSteers),
     "dampAngleSteersDes": float(LaC.dampened_desired_angle),
+    "dampRateSteersDes": float(LaC.dampened_desired_rate),
     "rateModeFF": LaC.rate_mode,
     "angleModeFF": LaC.angle_mode,
     "upSteer": float(LaC.pid.p),

selfdrive/controls/lib/latcontrol.py

@@ -60,7 +60,7 @@ def __init__(self, CP):
     self.actual_smoothing = max(1.0, CP.steerDampTime / _DT)
     self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT)
     self.ff_angle_factor = 1.0
-    self.ff_rate_factor = 0.0
+    self.ff_rate_factor = 1.0
     self.dampened_desired_angle = 0.0
     self.steer_counter = 1.0
     self.steer_counter_prev = 0.0
@@ -69,8 +69,8 @@ def __init__(self, CP):
     self.calculate_rate = True
     self.lane_prob_reset = False
     self.feed_forward = 0.0
-    self.rate_mode = 0
-    self.angle_mode = 1
+    self.rate_mode = 0.0
+    self.angle_mode = 0.0
 
     KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
     KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
@@ -128,27 +128,37 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
       self.pid.reset()
       self.dampened_angle_steers = float(angle_steers)
       self.dampened_desired_angle = float(angle_steers)
+      self.dampened_desired_rate = 0.0
+      self.dampened_rate_ff = 0.0
+      self.dampened_angle_ff = 0.0
     else:
 
       if self.gernbySteer == False:
         self.dampened_angle_steers = float(angle_steers)
         self.dampened_desired_angle = float(path_plan.angleSteers)
+        self.dampened_desired_rate = float(path_plan.rateSteers)
 
       else:
-        projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
+        cur_time = sec_since_boot()
+        projected_desired_angle = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
         self.dampened_desired_angle += ((projected_desired_angle - self.dampened_desired_angle) / self.desired_smoothing)
+        projected_desired_rate = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcRates)
+        self.dampened_desired_rate += ((projected_desired_rate - self.dampened_desired_rate) / self.desired_smoothing)
 
-        projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
         if not steer_override:
-          self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing)
+          projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
+        else:
+          projected_angle_steers = float(angle_steers)
+        self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing)
 
-      if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
+      '''if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
         if path_plan.lPoly[3] - path_plan.rPoly[3] > 3.9:
           print(self.dampened_desired_angle, path_plan.angleSteers)
           #self.dampened_desired_angle = path_plan.angleSteers
         self.lane_prob_reset = True
       elif path_plan.laneProb > 0.0:
         self.lane_prob_reset = False
+      '''
 
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
 
@@ -157,9 +167,24 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
         self.pid.neg_limit = -steers_max
         deadzone = 0.0
 
-        angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 0.5) * v_ego**2
-        rate_feedforward = self.ff_rate_factor * path_plan.rateSteers * v_ego**2
-        rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward))
+        angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 1.0) * v_ego**2
+        rate_feedforward = self.ff_rate_factor * self.dampened_desired_rate * v_ego**2
+
+        self.dampened_angle_ff += (angle_feedforward - self.dampened_angle_ff) / self.desired_smoothing
+        if angle_feedforward == 0 or (angle_feedforward < 0) == (rate_feedforward < 0):
+          self.dampened_rate_ff += (rate_feedforward - self.dampened_rate_ff) / self.desired_smoothing
+        else:
+          self.dampened_rate_ff += (0.0 - self.dampened_rate_ff) / self.desired_smoothing
+
+        feed_forward = self.dampened_angle_ff + self.dampened_rate_ff
+        if abs(feed_forward) > 0:
+          self.angle_mode = max(0.0, min(1.0, self.dampened_angle_ff / feed_forward))
+          self.rate_mode = 1.0 - self.angle_mode
+        else:
+          self.angle_mode = 0.0
+          self.rate_mode = 0.0
+
+        '''rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward))
         rate_same_direction = (rate_feedforward > 0) == (angle_feedforward > 0)
 
         if rate_more_significant and rate_same_direction: # and rate_away_from_zero:
@@ -171,9 +196,10 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
           self.rate_mode = 0
           self.angle_mode = 1
           self.feed_forward += ((angle_feedforward - self.feed_forward) / self.desired_smoothing)
+        '''
 
         output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
-                                      override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)
+                                      override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone)
 
     self.sat_flag = self.pid.saturated
     self.prev_angle_steers = float(angle_steers)

selfdrive/controls/lib/pathplanner.py

@@ -82,13 +82,17 @@ def update(self, CP, VM, CS, md, live100, live_parameters):
     if active:
       self.mpc_angles[0] = live100.live100.dampAngleSteersDes
       self.mpc_times[0] = live100.logMonoTime * 1e-9
+      self.mpc_rates[0] = live100.live100.dampRateSteersDes
       for i in range(1,20):
-        self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias)
-        self.mpc_rates[i] = math.degrees(self.mpc_solution[0].rate[i] * VM.sR)
         self.mpc_times[i] = self.mpc_times[i-1] + _DT_MPC
+        self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias)
+        self.mpc_rates[i] = (self.mpc_angles[i] - self.mpc_angles[i-1]) / (self.mpc_times[i] - self.mpc_times[i-1])
+        #self.mpc_rates[i] = math.degrees(self.mpc_solution[0].rate[i] * VM.sR)
 
       delta_desired = self.mpc_solution[0].delta[1]
       rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
+      rate_desired2 = math.degrees(self.mpc_solution[0].rate[1] * VM.sR)
+      print("%1.2f  %1.2f  %1.2f  %1.2f  %1.2f  %1.2f  " % (rate_desired, self.mpc_rates[1], rate_desired - self.mpc_rates[1], rate_desired2, self.mpc_rates[2], rate_desired2 - self.mpc_rates[2] ))
     else:
       #print(CP.steerRateCost, VM.sR)
       delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR