Use new locationd packet in paramsd

old-commit-hash: 01ac9d4722

selfdrive/locationd/paramsd.py

@@ -2,7 +2,6 @@
 import math
 
 import cereal.messaging as messaging
-import common.transformations.orientation as orient
 from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
 
 CARSTATE_DECIMATION = 5
@@ -22,14 +21,18 @@ class ParamsLearner:
     self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5
 
   def handle_log(self, t, which, msg):
-    if which == 'liveLocation':
-      roll, pitch, yaw = math.radians(msg.roll), math.radians(msg.pitch), math.radians(msg.heading)
-      v_device = orient.rot_from_euler([roll, pitch, yaw]).T.dot(msg.vNED)
+    if which == 'liveLocationKalman':
+
+      v_device = msg.velocityDevice.value
+      # v_device_std = msg.velocityDevice.std
       self.speed = v_device[0]
 
+      yaw_rate = msg.angularVelocityDevice.value[2]
+      # yaw_rate_std = msg.angularVelocityDevice.std[2]
+
       self.update_active()
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-msg.gyro[2]])
+        self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-yaw_rate])
         self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
 
         # Clamp values
@@ -59,7 +62,7 @@ class ParamsLearner:
 
 def main(sm=None, pm=None):
   if sm is None:
-    sm = messaging.SubMaster(['liveLocation', 'carState'])
+    sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
   if pm is None:
     pm = messaging.PubMaster(['liveParameters'])