need to do something about the valid (#1713)

selfdrive/locationd/locationd.py

@@ -7,7 +7,7 @@ import common.transformations.coordinates as coord
 from common.transformations.orientation import ecef_euler_from_ned, \
                                                euler_from_quat, \
                                                ned_euler_from_ecef, \
-                                               quat_from_euler, \
+                                               quat_from_euler, euler_from_rot, \
                                                rot_from_quat, rot_from_euler
 from rednose.helpers import KalmanError
 from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
@@ -81,6 +81,8 @@ class Localizer():
     #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
     orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
     orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
+    device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
+    calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef))
 
     acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
     acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
@@ -91,7 +93,6 @@ class Localizer():
       predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot(
         calib_from_device.T)))
 
-    device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
     vel_device = device_from_ecef.dot(vel_ecef)
     device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
     idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
@@ -133,6 +134,9 @@ class Localizer():
     fix.orientationECEF.value = to_float(orientation_ecef)
     fix.orientationECEF.std = to_float(orientation_ecef_std)
     fix.orientationECEF.valid = True
+    fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef)
+    #fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
+    #fix.calibratedOrientationECEF.valid = True
     fix.orientationNED.value = to_float(orientation_ned)
     #fix.orientationNED.std = to_float(orientation_ned_std)
     #fix.orientationNED.valid = True