Update latest changes from volvo-v0.8.6

Author: danielzmod

selfdrive/car/fw_versions.py

@@ -74,15 +74,13 @@ def p16(val):
 MAZDA_VERSION_RESPONSE =  bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
   p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
 
-# brand, request, response, response offset
 
 VOLVO_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
   p16(0xf1a2)
 VOLVO_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
   p16(0xf1a2)
 
-# supports subaddressing, request, response
-
+# brand, request, response, response offset
 REQUESTS = [
   # Hyundai
   (

selfdrive/car/volvo/carcontroller.py

@@ -152,8 +152,8 @@ def update(self, enabled, CS, frame,
       fingerprint = self.CP.carFingerprint
 
       if enabled and CS.out.vEgo > self.CP.minSteerSpeed:
-        current_steer_angle = CS.out.steeringAngle
-        self.SteerCommand.angle_request = actuators.steerAngle # Desired value from pathplanner
+        current_steer_angle = CS.out.steeringAngleDeg
+        self.SteerCommand.angle_request = actuators.steeringAngleDeg # Desired value from pathplanner
 
         # # windup slower
         if self.angle_request_prev * self.SteerCommand.angle_request > 0. and abs(self.SteerCommand.angle_request) > abs(self.angle_request_prev):
@@ -164,11 +164,11 @@ def update(self, enabled, CS, frame,
         self.SteerCommand.angle_request = clip(self.SteerCommand.angle_request, self.angle_request_prev - angle_rate_lim, self.angle_request_prev + angle_rate_lim)
 
         # Create trqlim from angle request (before constraints)
+        self.SteerCommand.trqlim = 0
         if fingerprint in PLATFORM.C1:
-          self.SteerCommand.trqlim = -127 if current_steer_angle > self.SteerCommand.angle_request else 127
+          #self.SteerCommand.trqlim = -127 if current_steer_angle > self.SteerCommand.angle_request else 127
           self.SteerCommand.steer_direction = CCP.STEER
         elif fingerprint in PLATFORM.EUCD:
-          self.SteerCommand.trqlim = 0
           # MIGHT be needed for EUCD
           self.SteerCommand.steer_direction = CCP.STEER_RIGHT if current_steer_angle > self.SteerCommand.angle_request else CCP.STEER_LEFT
           self.SteerCommand.steer_direction = self.dir_change(self.SteerCommand.steer_direction, current_steer_angle-self.SteerCommand.angle_request) # Filter the direction change 
@@ -177,7 +177,7 @@ def update(self, enabled, CS, frame,
         self.SteerCommand.steer_direction = CCP.STEER_NO
         self.SteerCommand.trqlim = 0
         if fingerprint in PLATFORM.C1:
-          self.SteerCommand.angle_request = clip(CS.out.steeringAngle, -359.95, 359.90)  # Cap values at max min values (Cap 2 steps from max min). Max=359.99445, Min=-360.0384 
+          self.SteerCommand.angle_request = clip(CS.out.steeringAngleDeg, -359.95, 359.90)  # Cap values at max min values (Cap 2 steps from max min). Max=359.99445, Min=-360.0384 
         else:
           self.SteerCommand.angle_request = 0
 

selfdrive/car/volvo/carstate.py

@@ -107,7 +107,7 @@ def update(self, cp, cp_cam):
     ret.standstill = ret.vEgoRaw < 0.1
 
      # Steering
-    ret.steeringAngle = cp.vl["PSCM1"]['SteeringAngleServo']
+    ret.steeringAngleDeg = cp.vl["PSCM1"]['SteeringAngleServo']
     ret.steeringTorque = 0 # Needed? No signal to check against yet
     ret.steeringPressed = bool(cp.vl["CCButtons"]['ACCSetBtn'] or \
       cp.vl["CCButtons"]['ACCMinusBtn'] or \
@@ -188,15 +188,15 @@ def update(self, cp, cp_cam):
     self.PSCMInfo.LKAActive = int(cp.vl['PSCM1']['LKAActive']) 
     self.PSCMInfo.SteeringAngleServo = float(cp.vl['PSCM1']['SteeringAngleServo']) 
 
-    # Platform specific  
+    # Platform specific
     if self.CP.carFingerprint in PLATFORM.C1:
       self.PSCMInfo.byte3 = int(cp.vl['PSCM1']['byte3']) 
     elif self.CP.carFingerprint in PLATFORM.EUCD:
       self.PSCMInfo.SteeringWheelRateOfChange = float(cp.vl['PSCM1']['SteeringWheelRateOfChange'])
 
     # FSMInfo
     # Common both platforms
-        
+
     if self.CP.carFingerprint in PLATFORM.C1:
       # TODO Why use these? In future shold be ok to delete.
       self.FSMInfo.TrqLim = int(cp_cam.vl['FSM1']['TrqLim']) 
@@ -241,7 +241,7 @@ def update(self, cp, cp_cam):
   def get_can_parser(CP):
     # ptcan on bus 0
     # this function generates lists for signal, messages and initial values
-    
+
     # Common signals for both platforms
     signals = [
       # sig_name, sig_address, default
@@ -253,15 +253,15 @@ def get_can_parser(CP):
       ("ACCMinusBtn", "CCButtons", 0),
       ("TimeGapIncreaseBtn", "CCButtons", 0),
       ("TimeGapDecreaseBtn", "CCButtons", 0),
-      
+
       # Common PSCM signals
       ("SteeringAngleServo", "PSCM1", 0),
       ("LKATorque", "PSCM1", 0),
       ("LKAActive", "PSCM1", 0),
       ("byte0", "PSCM1", 0),
       ("byte4", "PSCM1", 0),
       ("byte7", "PSCM1", 0),
-      
+
       # diagnostic
       ("byte03", "diagCEMResp", 0),
       ("byte47", "diagCEMResp", 0),
@@ -270,13 +270,17 @@ def get_can_parser(CP):
       ("byte03", "diagCVMResp", 0),
       ("byte47", "diagCVMResp", 0),
     ]
-   
+
     checks = [
       # sig_address, frequency
       ("CCButtons", 100),
-      ("PSCM1", 50),   
+      ("PSCM1", 50),
       ("VehicleSpeed1", 50),
       ("MiscCarInfo", 25),
+      ("diagCEMResp", 0),
+      ("diagPSCMResp", 0),
+      ("diagCVMResp", 0),
+
     ]
 
     # Car specific signals
@@ -288,26 +292,27 @@ def get_can_parser(CP):
       signals.append(("BrakePress1", "BrakeMessages", 0))
       signals.append(("BrakeStatus", "BrakeMessages", 0))
       signals.append(("GearShifter", "TCM0", 0))
-      
-      # Servo 
+
+      # Servo
       signals.append(("byte3", "PSCM1", 0))
 
       # Buttons
       signals.append(('ACCStopBtn', "CCButtons", 0))
-     
-      # Checks 
+
+      # Checks
       checks.append(("BrakeMessages", 50))
       checks.append(("ACC", 17))
       checks.append(("PedalandBrake", 100))
-    
+      checks.append(("TCM0", 10))
+
     if CP.carFingerprint in PLATFORM.EUCD:
       # Gas / Brake
       signals.append(("AccPedal", "AccPedal", 0))
       signals.append(("BrakePedal", "BrakePedal", 0))
 
       # Servo
       signals.append(("SteeringWheelRateOfChange", "PSCM1", 0))
-      
+ 
       # Buttons
       # Inv = Inverted state, init value set to passive
       signals.append(("ACCOnOffBtnInv", "CCButtons", 1))
@@ -340,7 +345,7 @@ def get_adas_can_parser(CP):
   @staticmethod
   def get_cam_can_parser(CP):
     # camera on bus 2
-    # Common signals 
+    # Common signals
     signals = [
       # sig_name, sig_address, default
       ("byte03", "diagFSMResp", 0),
@@ -350,8 +355,9 @@ def get_cam_can_parser(CP):
     # Common checks
     checks = [
       # sig_address, frequency
+      ("diagFSMResp", 0),
     ]
-    
+
     # Car specific
     if CP.carFingerprint in PLATFORM.C1:
       # LKA Request
@@ -372,12 +378,12 @@ def get_cam_can_parser(CP):
       # Checks
       checks.append(('FSM0', 100))
       checks.append(('FSM1', 50))
-    
+
     # TODO add checks and signals nescessary
     elif CP.carFingerprint in PLATFORM.EUCD:
       # ACC Status
       signals.append(("ACCStatus", "FSM0", 0))
-      
+
       # LKA Request
       signals.append(("TrqLim", "FSM2", 0x80))
       signals.append(("LKAAngleReq", "FSM2", 0x2000))
@@ -387,10 +393,10 @@ def get_cam_can_parser(CP):
       signals.append(("SET_X_02", "FSM2", 0x00))
       signals.append(("SET_X_10", "FSM2", 0x00))
       signals.append(("SET_X_A4", "FSM2", 0x00))
-      
+
       # Checks
       checks.append(('FSM0', 100))
       checks.append(('FSM2', 50))
 
-    
+
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)

selfdrive/car/volvo/interface.py

@@ -20,8 +20,8 @@ def compute_gb(accel, speed):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     # Volvo port is a community feature, since we don't own one to test
     ret.communityFeature = True
@@ -48,15 +48,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
     # Common parameters
     ret.carName = "volvo"
-    ret.enableCamera = True         # Will not set safety mode if not True
     ret.radarOffCan = True          # No radar objects on can
 
     # Steering settings - tuning parameters for lateral control.
     #ret.steerLimitAlert = True     # Do this do anything?
     ret.steerControlType = car.CarParams.SteerControlType.angle
     ret.minSteerSpeed = 1. * CV.KPH_TO_MS
     ret.steerRateCost = 1.          # Used in pathplanner for punishing? Steering derivative?
-    ret.steerActuatorDelay = 0.24   # Actuator delay from input to output.
+    ret.steerActuatorDelay = 0.48   # Actuator delay from input to output.
 
     # No PID control used. Set to a value, otherwise pid loop crashes.
     #ret.steerMaxBP = [0.] # m/s

selfdrive/car/volvo/values.py

@@ -36,29 +36,38 @@
 
 class CarControllerParams():
   # constants, collected from v40 dbc lka_direction.
+  # Normally the car uses STEER_RIGHT and STEER_LEFT.
+  # However it's possible to use STEER in C1MCA.
+  # Servo then accepts steering in both directions.
   STEER_NO = 0
   STEER_RIGHT = 1
   STEER_LEFT = 2
   STEER = 3
 
   # maximum degress offset/rate of change on request from current/last steering angle
-  MAX_ACT_ANGLE_REQUEST_DIFF = 25   # A bigger angle difference will trigger disengage.
-  STEER_ANGLE_DELTA_REQ_DIFF = 0.25
+  # Not used, old code
+  #MAX_ACT_ANGLE_REQUEST_DIFF = 3   # A bigger angle difference will trigger disengage.
+  #STEER_ANGLE_DELTA_REQ_DIFF = 0.25 # Not used for C1MCA
 
   # Limits  
-  ANGLE_DELTA_BP = [0., 5., 15., 27., 36.]   # 0, 18, 54, 97.2, 129.6 km/h
-  ANGLE_DELTA_V = [2, 1.2, .15, .1, .08]     # windup limit
-  ANGLE_DELTA_VU = [3, 1.8, 0.4, .2, .1]   # unwind limit
+  ANGLE_DELTA_BP = [0., 8.33, 13.89, 19.44, 25., 30.55, 36.1]   # 0, 30, 50, 70, 90, 110, 130 km/h
+  ANGLE_DELTA_V = [2., 1.2, .25, .20, .15, .10, .10]     # windup limit
+  ANGLE_DELTA_VU = [2., 1.2, .25, .20, .15, .10, .10]   # unwind limit
 
   # number of 0 torque samples in a row before trying to restore steering.
-  N_ZERO_TRQ = 10
+  # Got one false trigger on motorway with 10. 
+  # Increase to 12 is probably a good tradeoff between false triggers 
+  # and detecting fault.
+  N_ZERO_TRQ = 12
 
   # EUCD
-  # When changing steer direction steering request need to be blocked.
-  # This calibration sets the number of samples to block it and no steering instead.
+  # When changing steer direction steering request need to be blocked. 
+  # Otherwise servo wont "listen" to the request.
+  # This calibration sets the number of samples to block steering request.
   BLOCK_LEN = 8
-  # don't change steer direction inside deadzone, 
-  # might not be needed in future after discovering STEER command.
+  # When close to desired steering angle, don't change steer direction inside deadzone.
+  # Since we need to release control of the steering wheel for a brief moment, steering wheel will 
+  # unwind by itself. 
   DEADZONE = 0.1
 
 
@@ -77,17 +86,14 @@ class CAR:
   V40 = "VOLVO V40 2017"
   V60 = "VOLVO V60 2015"
 
-
 class PLATFORM:
   C1 = [CAR.V40]
   EUCD = [CAR.V60]
 
-
 ECU_ADDRESS = { 
   CAR.V40: {"BCM": 0x760, "ECM": 0x7E0, "DIM": 0x720, "CEM": 0x726, "FSM": 0x764, "PSCM": 0x730, "TCM": 0x7E1, "CVM": 0x793},
   }
 
-
 # TODO: Find good DID for identifying SW version
 # 0xf1a1, 0xf1a3 not on CVM.
 # Possible options 0xf1a2, 0xf1a4, 0xf1a5 
@@ -109,7 +115,6 @@ class PLATFORM:
   (Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31288595 AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'],            # 0xf1a2
   #(Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31678017\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'],  # 0xf1a4
   #(Ecu.eps, ECU_ADDRESS[CAR.V40]["PSCM"], None): [b'31681147 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'],           # 0xf1a5
-  #(Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31400454 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'],  # 0xf1a2
   (Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31400454 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'],  # 0xf1a2
   #(Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31660982 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a4
   #(Ecu.fwdCamera, ECU_ADDRESS[CAR.V40]["FSM"], None): [b'31660983 AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'], # 0xf1a5