Updated steer and mpc dampening

cereal/car.capnp

@@ -345,7 +345,9 @@ struct CarParams {
   longitudinalKpV @37 :List(Float32);
   longitudinalKiBP @38 :List(Float32);
   longitudinalKiV @39 :List(Float32);
-
+  steerMPCOffsetTime @51 :Float32;
+  steerMPCDampenTime @52 :Float32;
+  steerDampenTime @53 :Float32;
   steerLimitAlert @29 :Bool;
 
   vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
@@ -355,7 +357,7 @@ struct CarParams {
   steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
   steerControlType @46 :SteerControlType;
   radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN
-  syncID @51  :Int16;  # SyncID is optional
+  syncID @54  :Int16;  # SyncID is optional
 
   steerActuatorDelay @48 :Float32; # Steering wheel actuator delay in seconds
   openpilotLongitudinalControl @50 :Bool; # is openpilot doing the longitudinal control?

cereal/log.capnp

@@ -397,6 +397,8 @@ struct Live100Data {
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;     # Steering angle in degrees.
   angleSteersDes @29 :Float32;
+  dampAngleSteers @52 :Float32;
+  dampAngleSteersDes @53 :Float32;
   curvature @37 :Float32;       # path curvature from vehicle model
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
@@ -609,6 +611,8 @@ struct PathPlan {
   rProb @7 :Float32;
 
   angleSteers @8 :Float32;
+  mpcAngles @10 :List(Float32);
+  mpcTimes @11 :List(Float32);
   valid @9 :Bool;
 }
 

launch_chffrplus.sh

@@ -6,12 +6,14 @@ 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
-
+  '''
+
   # no cpu rationing for now
   echo 0-3 > /dev/cpuset/background/cpus
   echo 0-3 > /dev/cpuset/system-background/cpus

selfdrive/boardd/boardd.cc

@@ -524,7 +524,7 @@ void *can_recv_thread(void *crap) {
         }
       }
       else {
-        //force_send = (locked_wake_time > last_long_sleep);
+        force_send = (locked_wake_time > last_long_sleep + 1e5);
         wake_time += 1000;
         locked_wake_time = wake_time;
       }
@@ -749,7 +749,7 @@ int main() {
   LOGW("starting boardd");
 
   // set process priority
-  err = set_realtime_priority(3);
+  err = set_realtime_priority(1);
   LOG("setpriority returns %d", err);
 
   // check the environment

selfdrive/car/chrysler/interface.py

@@ -78,7 +78,10 @@ def get_params(candidate, fingerprint):
     ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
-
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
+
     if candidate == CAR.JEEP_CHEROKEE:
       ret.wheelbase = 2.91  # in meters
       ret.steerRatio = 12.7

selfdrive/car/ford/interface.py

@@ -77,7 +77,10 @@ def get_params(candidate, fingerprint):
     ret.steerKf = 1. / MAX_ANGLE   # MAX Steer angle to normalize FF
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
     ret.steerRateCost = 1.0
-
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
+
     f = 1.2
     tireStiffnessFront_civic *= f
     tireStiffnessRear_civic *= f

selfdrive/car/gm/interface.py

@@ -57,6 +57,9 @@ def get_params(candidate, fingerprint):
     ret.carFingerprint = candidate
 
     ret.enableCruise = False
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
 
     # Presence of a camera on the object bus is ok.
     # Have to go passive if ASCM is online (ACC-enabled cars),

selfdrive/car/honda/carstate.py

@@ -198,11 +198,11 @@ def update(self, cp, cp_cam):
                                       cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
     self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']
 
-    # 2 = temporary; 3 = TBD; 4 = temporary, hit a bump; 5 = (permanent); 6 = temporary; 7 = (permanent)
+    # 2 = temporary; 3 = TBD; 4 = ignore, steer override or bump; 5 = (permanent); 6 = temporary; 7 = (permanent)
     # TODO: Use values from DBC to parse this field
     self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6]
-    self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0
-    self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3]   # 3 is low speed lockout, not worth a warning
+    self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 4]
+    self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3, 4]   # 3 is low speed lockout, not worth a warning
     if self.CP.radarOffCan:
       self.brake_error = 0
     else:

selfdrive/car/honda/interface.py

@@ -175,6 +175,9 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
 
     # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle
     # model optimization process. Certain Hondas have an extra steering sensor at the bottom
@@ -216,6 +219,9 @@ def get_params(candidate, fingerprint):
       ret.steerRatio = 15.96  # 11.82 is spec end-to-end
       tire_stiffness_factor = 0.8467
       ret.syncID = 330
+      ret.steerMPCOffsetTime = 0.025
+      ret.steerMPCDampenTime = 0.3
+      ret.steerDampenTime = 0.3
       ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]

selfdrive/car/hyundai/carstate.py

@@ -185,7 +185,7 @@ 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']
+    self.angle_steers_rate = 0.0  # Don't use, since this is unsigned   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/hyundai/interface.py

@@ -68,7 +68,10 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
-
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
+
     ret.steerActuatorDelay = 0.1  # Default delay
     tire_stiffness_factor = 1.
 

selfdrive/car/mock/interface.py

@@ -61,7 +61,10 @@ def get_params(candidate, fingerprint):
     ret.tireStiffnessFront = 1e6    # very stiff to neglect slip
     ret.tireStiffnessRear = 1e6     # very stiff to neglect slip
     ret.steerRatioRear = 0.
-
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
+
     ret.steerMaxBP = [0.]
     ret.steerMaxV = [0.]  # 2/3rd torque allowed above 45 kph
     ret.gasMaxBP = [0.]

selfdrive/car/toyota/carstate.py

@@ -62,7 +62,7 @@ def get_can_parser(CP):
 
   if CP.carFingerprint == CAR.PRIUS:
     signals += [("STATE", "AUTOPARK_STATUS", 0)]
-  
+
   # add gas interceptor reading if we are using it
   if CP.enableGasInterceptor:
       signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
@@ -142,7 +142,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/car/toyota/interface.py

@@ -72,6 +72,9 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
+    ret.steerMPCOffsetTime = 0.025
+    ret.steerMPCDampenTime = 0.10
+    ret.steerDampenTime = 0.02
 
     ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
@@ -85,6 +88,9 @@ def get_params(candidate, fingerprint):
       ret.mass = 3045 * CV.LB_TO_KG + std_cargo
       ret.steerKpV, ret.steerKiV = [[0.4], [0.01]]
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
+      ret.steerMPCOffsetTime = 0.025
+      ret.steerMPCDampenTime = 0.20
+      ret.steerDampenTime = 0.2
       # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis
       ret.steerActuatorDelay = 0.25
 

selfdrive/controls/controlsd.py

@@ -257,7 +257,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
   # 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, CP, VM, path_plan)
 
   # Send a "steering required alert" if saturation count has reached the limit
@@ -336,6 +336,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "vEgo": CS.vEgo,
     "vEgoRaw": CS.vEgoRaw,
     "angleSteers": CS.steeringAngle,
+    "dampAngleSteers": float(LaC.dampened_angle_steers),
     "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
     "steerOverride": CS.steeringPressed,
     "state": state,
@@ -346,7 +347,8 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "upAccelCmd": float(LoC.pid.p),
     "uiAccelCmd": float(LoC.pid.i),
     "ufAccelCmd": float(LoC.pid.f),
-    "angleSteersDes": float(LaC.angle_steers_des),
+    "angleSteersDes": float(path_plan.pathPlan.angleSteers),
+    "dampAngleSteersDes": float(LaC.dampened_desired_angle),
     "upSteer": float(LaC.pid.p),
     "uiSteer": float(LaC.pid.i),
     "ufSteer": float(LaC.pid.f),
@@ -387,7 +389,7 @@ def controlsd_thread(gctx=None, rate=100):
   gc.disable()
 
   # start the loop
-  set_realtime_priority(4)
+  set_realtime_priority(3)
 
   context = zmq.Context()
   params = Params()
@@ -472,6 +474,7 @@ def controlsd_thread(gctx=None, rate=100):
 
   while True:
     start_time = int(sec_since_boot() * 1e9)
+    rk.monitor_time()
     prof.checkpoint("Ratekeeper", ignore=True)
 
     # Sample data and compute car events
@@ -480,7 +483,6 @@ def controlsd_thread(gctx=None, rate=100):
                   poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
                   state, mismatch_counter, params, plan, path_plan)
 
-    rk.monitor_time()
     prof.checkpoint("Sample")
 
     path_plan_age = (start_time - path_plan.logMonoTime) / 1e9

selfdrive/controls/lib/latcontrol.py

@@ -1,9 +1,9 @@
+from common.realtime import sec_since_boot
 from selfdrive.controls.lib.pid import PIController
 from common.numpy_fast import interp
 from cereal import car
 
 _DT = 0.01    # 100Hz
-_DT_MPC = 0.05  # 20Hz
 
 
 def get_steer_max(CP, v_ego):
@@ -12,35 +12,67 @@ def get_steer_max(CP, v_ego):
 
 class LatControl(object):
   def __init__(self, CP):
-    self.pid = PIController((CP.steerKpBP, CP.steerKpV),
-                            (CP.steerKiBP, CP.steerKiV),
+
+    self.mpc_frame = 0
+    self.actual_projection = CP.steerDampenTime
+    self.desired_projection = CP.steerMPCDampenTime
+    self.actual_smoothing = max(1.0, self.actual_projection / _DT)
+    self.desired_smoothing = max(1.0, (self.desired_projection - CP.steerMPCOffsetTime) / _DT)
+    self.dampened_angle_steers = 0.0
+    self.dampened_desired_angle = 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
+
+    KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
+    KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
+    self.pid = PIController(([0.], KpV),
+                            ([0.], KiV),
                             k_f=CP.steerKf, pos_limit=1.0)
-    self.last_cloudlog_t = 0.0
-    self.angle_steers_des = 0.
 
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM, path_plan):
+
+    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
+      self.prev_angle_steers = angle_steers
+    else:
+      # If non-zero angle_rate is provided, stop calculating rate
+      self.calculate_rate = False
+
     if v_ego < 0.3 or not active:
       output_steer = 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 = path_plan.angleSteers  # get from MPC/PathPlanner
-
-      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
+      projected_desired_angle = interp(sec_since_boot() + self.desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
+      self.dampened_desired_angle = (((self.desired_smoothing - 1.) * self.dampened_desired_angle) + projected_desired_angle) / self.desired_smoothing
+
       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)
+        projected_angle_steers = float(angle_steers) + self.actual_projection * float(angle_rate)
+        if not steer_override:
+          self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing
+
+        steers_max = get_steer_max(CP, v_ego)
+        self.pid.pos_limit = steers_max
+        self.pid.neg_limit = -steers_max
+        deadzone = 0.0
+
+        feed_forward = v_ego**2 * self.dampened_desired_angle
+        output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
+                                      override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone)
 
     self.sat_flag = self.pid.saturated
-    return output_steer, float(self.angle_steers_des)
+    self.prev_angle_steers = angle_steers
+
+    return output_steer, float(self.dampened_desired_angle)