new gernby 2

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);

launch_chffrplus.sh

@@ -6,11 +6,11 @@ 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
+  #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

opendbc/gm_global_a_powertrain.dbc

@@ -130,13 +130,15 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM
  SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 485 PSCMSteeringAngle: 8 K43_PSCM
- SG_ SteeringWheelAngle : 15|16@0- (0.0625,0) [-525|525] "deg"  NEO
+ SG_ SteeringWheelAngle : 15|16@0- (0.0625,-0.5) [-525|525] "deg"  NEO
  SG_ SteeringWheelRate : 27|12@0- (0.5,0) [-100|100] "deg/s"  NEO
 
 BO_ 489 EBCMVehicleDynamic: 8 K17_EBCM
- SG_ YawRate : 51|12@0- (0.0625,0) [-2047|2047] "grad/s"  NEO
- SG_ LateralAcceleration : 3|12@0- (0.0161,0) [-2047|2047] "m/s2"  NEO
- SG_ BrakePedalPressed : 6|1@0+ (1,0) [0|0] ""  NEO
+ SG_ BrakePedalPressed : 6|1@0+ (1,0) [0|0] "" NEO
+ SG_ NEW_SIGNAL_3 : 17|10@0- (1,0) [0|1] "" NEO
+ SG_ LateralAcceleration : 3|12@0- (0.0161,0) [-2047|2047] "m/s2" NEO
+ SG_ YawRate2 : 49|10@0- (0.0625,0) [-2047|2047] "grad/s" NEO
+ SG_ YawRate : 33|10@0- (0.0625,0) [-100|100] "grad/s" NEO
 
 BO_ 560 EPBStatus: 8 EPB
  SG_ EPBClosed : 12|1@0+ (1,0) [0|1] ""  NEO

panda/board/safety/safety_gm.h

@@ -8,8 +8,8 @@
 //      brake rising edge
 //      brake > 0mph
 
-const int GM_MAX_STEER = 300;
-const int GM_MAX_RT_DELTA = 128;          // max delta torque allowed for real time checks
+const int GM_MAX_STEER = 400;
+const int GM_MAX_RT_DELTA = 200;          // max delta torque allowed for real time checks
 const int32_t GM_RT_INTERVAL = 250000;    // 250ms between real time checks
 const int GM_MAX_RATE_UP = 8;
 const int GM_MAX_RATE_DOWN = 20;

selfdrive/car/ford/interface.py

@@ -77,6 +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.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     f = 1.2
     tireStiffnessFront_civic *= f

selfdrive/car/gm/carcontroller.py

@@ -11,10 +11,10 @@
 class CarControllerParams():
   def __init__(self, car_fingerprint):
     if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
-      self.STEER_MAX = 300
+      self.STEER_MAX = 400
       self.STEER_STEP = 2              # how often we update the steer cmd
-      self.STEER_DELTA_UP = 8          # ~0.75s time to peak torque (255/50hz/0.75s)
-      self.STEER_DELTA_DOWN = 20       # ~0.3s from peak torque to zero
+      self.STEER_DELTA_UP = 7          # ~0.75s time to peak torque (255/50hz/0.75s)
+      self.STEER_DELTA_DOWN = 17       # ~0.3s from peak torque to zero
     elif car_fingerprint == CAR.CADILLAC_CT6:
       self.STEER_MAX = 150
       self.STEER_STEP = 1              # how often we update the steer cmd

selfdrive/car/gm/carstate.py

@@ -31,6 +31,7 @@ def get_powertrain_can_parser(CP, canbus):
     ("LKADriverAppldTrq", "PSCMStatus", 0),
     ("LKATorqueDeliveredStatus", "PSCMStatus", 0),
     ("DistanceButton", "ASCMSteeringButton", 0),
+    ("YawRate", "EBCMVehicleDynamic", 0),
   ]
 
   if CP.carFingerprint == CAR.VOLT:

selfdrive/car/gm/interface.py

@@ -103,7 +103,7 @@ def get_params(candidate, fingerprint):
       ret.mass = 4353. * CV.LB_TO_KG + std_cargo
       ret.safetyModel = car.CarParams.SafetyModels.gm
       ret.wheelbase = 2.86
-      ret.steerRatio = 14.4  #end to end is 13.46
+      ret.steerRatio = 12.0  #end to end is 13.46
       ret.steerRatioRear = 0.
       ret.centerToFront = ret.wheelbase * 0.4
 
@@ -183,8 +183,13 @@ def get_params(candidate, fingerprint):
 
     elif candidate == CAR.ACADIA:
       ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
-      ret.steerKpV, ret.steerKiV = [[0.63], [0.1]]
-      ret.steerKf = 0.00006  # full torque for 20 deg at 80mph means 0.00007818594
+      ret.steerKpV, ret.steerKiV = [[0.4], [0.1]]
+      ret.steerKf = 0.00003  # full torque for 20 deg at 80mph means 0.00007818594
+
+      ret.steerReactance = 0.9
+      ret.steerInductance = 1.0
+      ret.steerResistance = 1.0
+      ret.eonToFront = 1.2
 
       ret.steerMaxBP = [0.] # m/s
       ret.steerMaxV = [1.]
@@ -205,7 +210,7 @@ def get_params(candidate, fingerprint):
       ret.stoppingControl = True
       ret.startAccel = 1.0
       ret.steerActuatorDelay = 0.15  # Default delay, not measured yet
-      ret.steerRateCost = 1.0
+      ret.steerRateCost = 0.5
 
     ret.steerControlType = car.CarParams.SteerControlType.torque
 
@@ -388,4 +393,4 @@ def apply(self, c, perception_state=log.Live20Data.new_message()):
       c.hudControl.leadVisible, \
       chime, chime_count)
 
-    self.frame += 1
+    self.frame += 1 

selfdrive/car/honda/carcontroller.py

@@ -138,7 +138,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/carstate.py

@@ -36,7 +36,6 @@ 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),
@@ -240,15 +239,8 @@ 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']
-
-    # Apply the reported angle offset for cars that have it defined
-    if self.CP.carFingerprint in  (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH):
-      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 = 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']
     self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
 

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/interface.py

@@ -69,6 +69,11 @@ def get_params(candidate, fingerprint):
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
 
+    ret.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
+
     ret.steerActuatorDelay = 0.1  # Default delay
     tire_stiffness_factor = 1.
 

selfdrive/car/mock/interface.py

@@ -74,6 +74,10 @@ def get_params(candidate, fingerprint):
     ret.longitudinalKiBP = [0.]
     ret.longitudinalKiV = [0.]
     ret.steerActuatorDelay = 0.
+    ret.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     return ret
 

selfdrive/car/toyota/interface.py

@@ -76,6 +76,11 @@ def get_params(candidate, fingerprint):
     ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
 
+    ret.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
+
     if candidate == CAR.PRIUS:
       ret.safetyParam = 66  # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
@@ -86,6 +91,7 @@ def get_params(candidate, fingerprint):
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
       # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis
       ret.steerActuatorDelay = 0.25
+      ret.eonToFront = -1.0
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       ret.safetyParam = 73  # see conversion factor for STEER_TORQUE_EPS in dbc file

selfdrive/controls/controlsd.py

@@ -280,7 +280,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
                                                      CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)
 
   # Send a "steering required alert" if saturation count has reached the limit
-  if LaC.sat_flag and CP.steerLimitAlert and CS.lkMode and not CS.leftBlinker and not CS.rightBlinker:
+  if LaC.sat_flag and CP.steerLimitAlert and not CS.leftBlinker and not CS.rightBlinker:
     AM.add("steerSaturated", enabled)
 
   # Parse permanent warnings to display constantly
@@ -549,6 +549,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
           CP.steerRatio = tuning.steerRatio[0]
           CP.steerActuatorDelay = tuning.steerActuatorDelay[0]
           CP.steerRateCost = tuning.steerRateCost[0]
+          CP.steerReactance = tuning.steerReactance[0]
+          CP.steerInductance = tuning.steerInductance[0]
+          CP.steerResistance = tuning.steerResistance[0]
+          CP.eonToFront = tuning.eonToFront[0]
 
           last_mod_time = os.path.getmtime(tune_file)
         else:
@@ -562,6 +566,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
         print "CP.steerRatio: %s" % CP.steerRatio
         print "CP.steerActuatorDelay: %s" % CP.steerActuatorDelay
         print "CP.steerRateCost: %s" % CP.steerRateCost
+        print "CP.steerReactance: %s" % CP.steerReactance
+        print "CP.steerInductance: %s" % CP.steerInductance
+        print "CP.steerResistance: %s" % CP.steerResistance
+        print "CP.eonToFront: %s" % CP.eonToFront
 
         VM.update_rt_params(CP)
         LaC.update_rt_params(CP)

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 + 1.3
+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
 
@@ -35,29 +35,26 @@ def apply_deadzone(angle, deadzone):
 class LatControl(object):
   def __init__(self, CP):
 
-    _ADJUST_REACTANCE = 1.0
-    _ADJUST_INDUCTANCE = 1.2
-    _ADJUST_RESISTANCE = 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) * _ADJUST_REACTANCE]
-    KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE]
-    Kf = CP.steerKf * _ADJUST_INDUCTANCE
+    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 = _ADJUST_INDUCTANCE * 2.0 * CP.steerActuatorDelay / _DT    # Multiplier for inductive component (feed forward)
-    self.projection_factor = _ADJUST_REACTANCE * 5.0 * _DT                         #  Mutiplier for reactive component (PI)
-    self.accel_limit = 2.0 / _ADJUST_RESISTANCE                                    # Desired acceleration limit to prevent "whip steer" (resistive component)
+    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
@@ -72,6 +69,9 @@ def update_rt_params(self, CP):
     #KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE]
     #Kf = CP.steerKf * _ADJUST_INDUCTANCE
     self.setup_mpc(CP.steerRateCost)
+    self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT
+    self.projection_factor = CP.steerReactance * 5.0 * _DT
+    self.accel_limit = 2.0 / CP.steerResistance     
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -97,7 +97,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
     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 = 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev
+        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)
@@ -120,19 +120,19 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
       curvature_factor = VM.curvature_factor(v_ego)
 
       # Determine future angle steers using steer rate
-      projected_angle_steers = float(angle_steers) + CP.steerActuatorDelay * float(angle_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 = cur_time - mpc_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 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)
+      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,
@@ -151,6 +151,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
         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

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.05  # m from center car to camera
+CAMERA_OFFSET = 0.06  # m from center car to camera
 
 class PathPlanner(object):
   def __init__(self):
@@ -12,9 +12,9 @@ def __init__(self):
     self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
     self._path_pinv = compute_path_pinv()
 
-    self.lane_width_estimate = 3.7
+    self.lane_width_estimate = 3.2
     self.lane_width_certainty = 1.0
-    self.lane_width = 3.7
+    self.lane_width = 3.2
 
   def update(self, v_ego, md):
     if md is not None: