Merge pull request commaai#76 from Gernby/resonant-interp-arne

updated resonant steering. Looks good for testing. Thanks.

cereal/car.capnp

@@ -334,6 +334,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
@@ -348,6 +349,9 @@ 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);

selfdrive/car/ford/interface.py

@@ -77,7 +77,11 @@ 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
     tireStiffnessRear_civic *= f

selfdrive/car/gm/interface.py

@@ -63,6 +63,10 @@ def get_params(candidate, fingerprint):
     ret.carFingerprint = candidate
 
     ret.enableCruise = False
+    ret.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     # Presence of a camera on the object bus is ok.
     # Have to go passive if ASCM is online (ACC-enabled cars),

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
@@ -227,6 +231,10 @@ def get_params(candidate, fingerprint):
       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.75
+      ret.steerInductance = 2.25
+      ret.steerResistance = 0.5
+      ret.eonToFront = 1.0
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]
@@ -312,9 +320,9 @@ def get_params(candidate, fingerprint):
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
       ret.longitudinalKiBP = [0., 35.]
       ret.longitudinalKiV = [0.18, 0.12]
-      #tire_stiffness_factor = 0.444 # not optimized yet  
+      #tire_stiffness_factor = 0.444 # not optimized yet
       tire_stiffness_factor = 0.82  # trying same as odyssey
-      #ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] #original model: OS 30%, risetime: 1.75s, Max Output 0.8 of 1  
+      #ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] #original model: OS 30%, risetime: 1.75s, Max Output 0.8 of 1
       #ret.steerKpV, ret.steerKiV = [[0.38], [0.23]] #model: OS 10%,risetime 2.75s - RESULT: Better than default stock values.
       #ret.steerKpV, ret.steerKiV = [[0.38], [0.3]]  #model: OS 5%, risetime 3.5s  - NOT TESTED
       #ret.steerKpV, ret.steerKiV = [[0.38], [0.25]] #model: OS 7%, risetime 3s    - NOT TESTED
@@ -324,13 +332,13 @@ def get_params(candidate, fingerprint):
       #ret.steerKpV, ret.steerKiV = [[0.5], [0.24]]  #tweaking optimal result      - RESULT: Does not fix slow lane hug
       #ret.steerKpV, ret.steerKiV = [[0.5], [0.3]]   #model: OS 3%, risetime 3.5s  - NOT TESTED
       #ret.steerKpV, ret.steerKiV = [[0.8], [0.23]]  #model: OS 5%, risetime 3s    - NOT TESTED
-      #ret.steerKpV, ret.steerKiV = [[0.8], [0.3]]   #model: OS 2%, risetime 3.8s  - RESULT: Fast lane left hugging recurrence. Wheel jiggles after fast turn.     
+      #ret.steerKpV, ret.steerKiV = [[0.8], [0.3]]   #model: OS 2%, risetime 3.8s  - RESULT: Fast lane left hugging recurrence. Wheel jiggles after fast turn.
       #ret.steerKpV, ret.steerKiV = [[1], [0.14]]    #model: OS 18%,risetime 2s    - NOT TESTED - CAUTION Large kP
       #ret.steerKpV, ret.steerKiV = [[1.5], [0.2]]   #model: OS 5%, risetime 2.3s  - NOT TESTED - CAUTION Large kP
       #ret.steerKpV, ret.steerKiV = [[2], [0.24]]    #model: OS 0%, risetime 3s    - NOT TESTED - CAUTION Large kP
       #ret.steerKpV, ret.steerKiV = [[2], [0.21]]    #model: OS 3%, risetime 2.3s  - NOT TESTED - CAUTION Large kP
       #ret.steerKf = 0.00009 # - NOT TESTED
-      #ret.steerKf = 0.00012 # - NOT TESTED - RESULT: 0.5,0.23 makes fast lane hug left side 
+      #ret.steerKf = 0.00012 # - NOT TESTED - RESULT: 0.5,0.23 makes fast lane hug left side
       #ret.steerKf = 0.00015 # - NOT TESTED
       #ret.steerKf = 0.00018 # - NOT TESTED
       #ret.steerKf = 0.00021 # - NOT TESTED
@@ -455,7 +463,7 @@ def update(self, c):
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
     ret.cruiseState.standstill = False
-    
+
     ret.readdistancelines = self.CS.read_distance_lines
 
     # TODO: button presses

selfdrive/car/hyundai/carstate.py

@@ -146,19 +146,19 @@ def __init__(self, CP):
     self.CL_LANE_DETECT_BP = [10., 44.]
     self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
     self.CL_LANE_PASS_BP = [10., 20., 44.]
-    self.CL_LANE_PASS_TIME = [40.,10., 3.] 
+    self.CL_LANE_PASS_TIME = [40.,10., 3.]
      # change lane delta angles and other params
     self.CL_MAXD_BP = [10., 32., 44.]
     self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
     self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
      # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
     self.CL_MAX_A_BP = [10., 44.]
-    self.CL_MAX_A = [10., 10.] 
+    self.CL_MAX_A = [10., 10.]
      # define limits for angle change every 0.1 s
     # we need to force correction above 10 deg but less than 20
     # anything more means we are going to steep or not enough in a turn
     self.CL_MAX_ACTUATOR_DELTA = 2.
-    self.CL_MIN_ACTUATOR_DELTA = 0. 
+    self.CL_MIN_ACTUATOR_DELTA = 0.
     self.CL_CORRECTION_FACTOR = [1.3,1.2,1.2]
     self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
      #duration after we cross the line until we release is a factor of speed
@@ -167,7 +167,7 @@ def __init__(self, CP):
     #duration to wait (in seconds) with blinkers on before starting to turn
     self.CL_WAIT_BEFORE_START = 1
     #END OF ALCA PARAMS
-    
+
     self.CP = CP
 
     #BB UIEvents
@@ -181,13 +181,13 @@ def __init__(self, CP):
 
     #BB custom message counter
     self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
-    
+
     # initialize can parser
     self.car_fingerprint = CP.carFingerprint
 
 
 
-    
+
     # vEgo kalman filter
     dt = 0.01
     # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
@@ -228,7 +228,7 @@ def update_ui_buttons(self,id,btn_status):
     else:
         self.cstm_btns.btns[id].btn_status = btn_status
 
-    
+
   def update(self, cp, cp_cam):
     # copy can_valid
     self.can_valid = cp.can_valid
@@ -271,7 +271,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  # 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

@@ -71,6 +71,10 @@ def get_params(candidate, fingerprint):
 
     ret.steerActuatorDelay = 0.1  # Default delay
     tire_stiffness_factor = 1.
+    ret.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     if candidate == CAR.SANTA_FE:
       ret.steerKf = 0.00005
@@ -196,7 +200,7 @@ def update(self, c):
       ret.gearShifter = self.CS.gear_shifter_cluster
     else:
       ret.gearShifter = self.CS.gear_shifter
-    
+
     ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas")
     ret.readdistancelines = self.CS.read_distance_lines
     # gas pedal

selfdrive/car/mock/interface.py

@@ -61,6 +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.steerReactance = 0.7
+    ret.steerInductance = 1.0
+    ret.steerResistance = 1.0
+    ret.eonToFront = 0.5
 
     ret.steerMaxBP = [0.]
     ret.steerMaxV = [0.]  # 2/3rd torque allowed above 45 kph

selfdrive/car/toyota/carstate.py

@@ -29,7 +29,7 @@ def gps_distance(gpsLat, gpsLon, gpsAlt, gpsAcc):
   includeradius = B[minindex,5]
   approachradius = B[minindex,6]
   speedlimit = float(B[minindex,7])
-  
+
   if abs(gpsAlt -B[minindex,3]) < altacc:
     if gpsAcc<1.00001:
       #dist = 6371010*acos(sin(radians(gpsLat))*sin(radians(lat))+cos(radians(gpsLat))*cos(radians(lat))*cos(radians(gpsLon-lon)))
@@ -109,7 +109,7 @@ def get_can_parser(CP):
   if CP.enableGasInterceptor:
     signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
     checks.append(("GAS_SENSOR", 50))
-    
+
   return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
@@ -148,19 +148,19 @@ def __init__(self, CP):
     self.CL_LANE_DETECT_BP = [10., 44.]
     self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
     self.CL_LANE_PASS_BP = [10., 20., 44.]
-    self.CL_LANE_PASS_TIME = [40.,10., 3.] 
+    self.CL_LANE_PASS_TIME = [40.,10., 3.]
      # change lane delta angles and other params
     self.CL_MAXD_BP = [10., 32., 44.]
     self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
     self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
      # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
     self.CL_MAX_A_BP = [10., 44.]
-    self.CL_MAX_A = [10., 10.] 
+    self.CL_MAX_A = [10., 10.]
      # define limits for angle change every 0.1 s
     # we need to force correction above 10 deg but less than 20
     # anything more means we are going to steep or not enough in a turn
     self.CL_MAX_ACTUATOR_DELTA = 2.
-    self.CL_MIN_ACTUATOR_DELTA = 0. 
+    self.CL_MIN_ACTUATOR_DELTA = 0.
     self.CL_CORRECTION_FACTOR = [1.3,1.2,1.2]
     self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
      #duration after we cross the line until we release is a factor of speed
@@ -169,7 +169,7 @@ def __init__(self, CP):
     #duration to wait (in seconds) with blinkers on before starting to turn
     self.CL_WAIT_BEFORE_START = 1
     #END OF ALCA PARAMS
-    
+
     context = zmq.Context()
     self.poller = zmq.Poller()
     self.lastlat_Control = None
@@ -254,7 +254,7 @@ def update(self, cp, cp_cam):
         msg = messaging.recv_one(socket)
       elif socket is self.lat_Control:
         self.lastlat_Control = messaging.recv_one(socket).latControl
-    
+
     if msg is not None:
       gps_pkt = msg.gpsLocationExternal
       self.inaccuracy = gps_pkt.accuracy
@@ -299,8 +299,12 @@ def update(self, cp, cp_cam):
     #self.standstill = not self.v_wheel > 0.001
     self.standstill = False
 
+    # 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
     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']
     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']
@@ -318,7 +322,7 @@ def update(self, cp, cp_cam):
       self.blind_spot_on = bool(1)
     else:
       self.blind_spot_on = bool(0)
-    
+
     # we could use the override bit from dbc, but it's triggered at too high torque values
     self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
 
@@ -341,7 +345,7 @@ def update(self, cp, cp_cam):
         self.lane_departure_toggle_on = False
       else:
         self.lane_departure_toggle_on = True
-      
+
     self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
     self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
     if self.distance_toggle <> self.distance_toggle_prev:
@@ -358,7 +362,7 @@ def update(self, cp, cp_cam):
       if self.read_distance_lines == 3:
         self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
       self.read_distance_lines_prev = self.read_distance_lines
-    
+
     if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
       self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
       if self.acc_slow_on:
@@ -399,7 +403,7 @@ def update(self, cp, cp_cam):
       print "inside"
       if self.v_cruise_pcm > self.speedlimit:
         self.v_cruise_pcm =  self.speedlimit
-    
+
     self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
     self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
     self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
@@ -409,4 +413,4 @@ def update(self, cp, cp_cam):
       self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
     else:
       self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
-    
+

selfdrive/car/toyota/interface.py

@@ -74,6 +74,10 @@ 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:
       stop_and_go = True
@@ -82,6 +86,7 @@ def get_params(candidate, fingerprint):
       ret.steerRatio = 15.00   # unknown end-to-end spec
       tire_stiffness_factor = 1.0
       ret.mass = 3370 * CV.LB_TO_KG + std_cargo
+      ret.eonToFront = 0.0
       ret.steerKpV, ret.steerKiV = [[0.369,0.397,0.418], [0.0056,0.0093,0.0121]]
       ret.steerKf = 0.000078   # 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
@@ -232,12 +237,12 @@ def get_params(candidate, fingerprint):
 
     #detect the Pedal address
     ret.enableGasInterceptor = 0x201 in fingerprint
-    
-    
+
+
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
-    
+
     centerToRear = ret.wheelbase - ret.centerToFront
     # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
@@ -280,7 +285,7 @@ def get_params(candidate, fingerprint):
 
     ret.longitudinalKpBP = [0., 5., 35.]
     ret.longitudinalKiBP = [0., 35.]
-    
+
     return ret
 
   # returns a car.CarState
@@ -376,7 +381,7 @@ def update(self, c):
     ret.accSlowToggle = self.CS.acc_slow_on
     ret.readdistancelines = self.CS.read_distance_lines
     ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas")
-    
+
     # events
     events = []
     if not self.CS.can_valid:
@@ -445,7 +450,7 @@ def apply(self, c, perception_state=log.Live20Data.new_message()):
 
     self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
                    c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
-                   c.hudControl.audibleAlert, self.forwarding_camera, c.hudControl.leftLaneVisible, 
+                   c.hudControl.audibleAlert, self.forwarding_camera, c.hudControl.leftLaneVisible,
                    c.hudControl.rightLaneVisible, c.hudControl.leadVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
 
     self.frame += 1