From 21ccf4a6fd8330566e1bc3379c40feddfc5b1218 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 1 Aug 2022 22:06:58 -0700 Subject: [PATCH] Quectel unix timestamp (#25329) * Use laika * Fix bug * Better timestamp name * Better name * bump cereal old-commit-hash: 32201bbbbda0ce4c2ff8cad81ecfe45ca15e4f81 --- cereal | 2 +- selfdrive/locationd/locationd.cc | 2 +- selfdrive/locationd/ublox_msg.cc | 2 +- selfdrive/sensord/rawgps/rawgpsd.py | 5 ++--- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/cereal b/cereal index a4db5e79e4..fbd45de6e6 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit a4db5e79e4fac20a906bd951f75b1b91c06148b5 +Subproject commit fbd45de6e6bc71b4561eaef65dd86fce952c5d55 diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index ffdd649a84..7714931b94 100755 --- a/selfdrive/locationd/locationd.cc +++ b/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_START) - ecef_pos).norm(); VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index c9833410e7..c9f732e9ab 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -144,7 +144,7 @@ kj::Array 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); diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 14f0fe3878..e3336f4870 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/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.timestamp = report['w_GpsWeekNumber']*604800*1000 + report['q_GpsFixTimeMs'] + gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'], 1e-3*report['q_GpsFixTimeMs']).as_datetime().timestamp()*1e3 gps.source = log.GpsLocationData.SensorSource.qcomdiag gps.vNED = vNED gps.verticalAccuracy = report["q_FltVdop"]