Quectel unix timestamp (#25329)

* Use laika

* Fix bug

* Better timestamp name

* Better name

* bump cereal
old-commit-hash: 32201bbbbd

cereal

@@ -1 +1 @@
-Subproject commit a4db5e79e4fac20a906bd951f75b1b91c06148b5
+Subproject commit fbd45de6e6bc71b4561eaef65dd86fce952c5d55

selfdrive/locationd/locationd.cc

@@ -284,7 +284,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
   MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
 
-  this->unix_timestamp_millis = log.getTimestamp();
+  this->unix_timestamp_millis = log.getUnixTimestampMillis();
   double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
 
   VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));

selfdrive/locationd/ublox_msg.cc

@@ -144,7 +144,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
   timeinfo.tm_sec = msg->sec();
 
   std::time_t utc_tt = timegm(&timeinfo);
-  gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano() * 1e-06);
+  gpsLoc.setUnixTimestampMillis(utc_tt * 1e+03 + msg->nano() * 1e-06);
   float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f };
   gpsLoc.setVNED(f);
   gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03);

selfdrive/sensord/rawgps/rawgpsd.py

@@ -7,10 +7,10 @@ import math
 import time
 from typing import NoReturn
 from struct import unpack_from, calcsize, pack
-
 import cereal.messaging as messaging
 from cereal import log
 from system.swaglog import cloudlog
+from laika.gps_time import GPSTime
 
 from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
 from selfdrive.sensord.rawgps.structs import dict_unpacker
@@ -211,8 +211,7 @@ def main() -> NoReturn:
       gps.altitude = report["q_FltFinalPosAlt"]
       gps.speed = math.sqrt(sum([x**2 for x in vNED]))
       gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
-      # TODO: this probably isn't right, use laika for this
+      gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'], 1e-3*report['q_GpsFixTimeMs']).as_datetime().timestamp()*1e3
-      gps.timestamp = report['w_GpsWeekNumber']*604800*1000 + report['q_GpsFixTimeMs']
       gps.source = log.GpsLocationData.SensorSource.qcomdiag
       gps.vNED = vNED
       gps.verticalAccuracy = report["q_FltVdop"]