Fixed PCC vehicle ref speed (commaai#141)

* fixed PCC vehicle ref speed (wasn't based on the exact speed shown in IC but an approximation)

* fixed rounding issue for acc/pcc speed in IC when adjusting by double tapping

selfdrive/car/tesla/carcontroller.py

@@ -372,7 +372,7 @@ def update(self, enabled, CS, frame, actuators, \
     elif self.PCC.enable_pedal_cruise:
       CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph
     else:
-      CS.v_cruise_pcm = max(0.,CS.v_ego * CV.MS_TO_KPH  +0.5) * speed_uom_kph
+      CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph
     # Get the turn signal from ALCA.
     turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame)
     if turn_signal_needed == 0:
@@ -418,7 +418,7 @@ def update(self, enabled, CS, frame, actuators, \
     # DAS_speed_kph(8), 
     # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3), 
     # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
-    # DAS_acc_speed_limit_mph (8), 
+    # DAS_acc_speed_limit (8),
     # DAS_speed_limit_units(8)
     #send fake_das data as 0x553
     # TODO: forward collission warning
@@ -489,7 +489,6 @@ def update(self, enabled, CS, frame, actuators, \
 
     speed_override = 0
     collision_warning = 0x00
-    acc_speed_limit_mph = 0
     speed_control_enabled = 0
     accel_min = -15
     accel_max = 5
@@ -536,9 +535,7 @@ def update(self, enabled, CS, frame, actuators, \
           hands_on_state = 0x03
         else:
           hands_on_state = 0x05
-      acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1)
       if self.PCC.pcc_available:
-        acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1)
         acc_speed_kph = self.PCC.pedal_speed_kph
       if hud_alert == AH.FCW:
         collision_warning = hud_alert[1]
@@ -619,7 +616,7 @@ def update(self, enabled, CS, frame, actuators, \
             acc_speed_kph, \
             turn_signal_needed,forward_collision_warning, adaptive_cruise,  hands_on_state, \
             cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \
-            CS.v_cruise_pcm, #acc_speed_limit_mph,
+            CS.v_cruise_pcm,
             CS.speedLimitToIc, #speed_limit_to_car,
             apply_angle,
             1 if enable_steer_control else 0,

selfdrive/car/tesla/carstate.py

@@ -64,6 +64,7 @@ def get_can_signals(CP):
       ("LgtSens_Night", "BODY_R1", 0),
       ("DI_torqueMotor", "DI_torque1",0),
       ("DI_speedUnits", "DI_state", 0),
+      ("DI_analogSpeed", "DI_state", 0),
       # Steering wheel stalk signals (useful for managing cruise control)
       ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0),
       ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0),
@@ -590,14 +591,14 @@ def update(self, cp, epas_cp, pedal_cp):
     self.v_wheel_rr = 0 #JCT
     self.v_wheel = 0 #JCT
     self.v_weight = 0 #JCT
-    speed = (cp.vl["DI_torque2"]['DI_vehicleSpeed']) * CV.MPH_TO_KPH/3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S
-    speed = speed * 1.02 # To match car's displayed speed
+    self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0
+    speed_ms = cp.vl["DI_state"]['DI_analogSpeed'] * (CV.MPH_TO_MS if self.imperial_speed_units else CV.KPH_TO_MS) # car's displayed speed in m/s
 
-    if abs(speed - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
-      self.v_ego_kf.x = [[speed], [0.0]]
+    if abs(speed_ms - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
+      self.v_ego_kf.x = [[speed_ms], [0.0]]
 
-    self.v_ego_raw = speed
-    v_ego_x = self.v_ego_kf.update(speed)
+    self.v_ego_raw = speed_ms
+    v_ego_x = self.v_ego_kf.update(speed_ms)
     self.v_ego = float(v_ego_x[0])
     self.a_ego = float(v_ego_x[1])
 
@@ -628,7 +629,6 @@ def update(self, cp, epas_cp, pedal_cp):
     self.brake_hold = 0  # TODO
 
     self.main_on = 1 #cp.vl["SCM_BUTTONS"]['MAIN_ON']
-    self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0
     self.DI_cruiseSet = cp.vl["DI_state"]['DI_cruiseSet']
     if self.imperial_speed_units:
       self.DI_cruiseSet = self.DI_cruiseSet * CV.MPH_TO_KPH

selfdrive/car/tesla/teslacan.py

@@ -142,7 +142,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll
                  acc_speed_kph, \
                  turn_signal_needed,forward_collission_warning,adaptive_cruise, hands_on_state, \
                  cc_state, pcc_available, alca_state, \
-                 acc_speed_limit_mph,
+                 acc_speed_limit, # IC cruise speed, kph or mph
                  legal_speed_limit,
                  apply_angle,
                  enable_steer_control, 
@@ -156,7 +156,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll
       int(acc_speed_kph), \
       int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4)  + (adaptive_cruise << 3) + hands_on_state), \
       int((cc_state << 6) + (pcc_available << 5) + alca_state), \
-      int(acc_speed_limit_mph),
+      int(acc_speed_limit + 0.5), # IC rounds current speed, so we need to round cruise speed the same way
       int((legal_speed_limit & 0x1F) + ((park_brake_request << 5) & 0x20)), #positions 7 and 6 not used yet
       int(c_apply_steer & 0xFF),
       int((c_apply_steer >> 8) & 0xFF))