diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 1eca908982..e2d248ab51 100755 --- a/selfdrive/locationd/paramsd.py +++ b/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'])