mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-19 06:34:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quectel unix timestamp (#25329)

Browse Source 
* Use laika

* Fix bug

* Better timestamp name

* Better name

* bump cereal
This commit is contained in:
HaraldSchafer
2022-08-01 22:06:58 -07:00
committed by GitHub
parent 772b748689
commit 32201bbbbd
4 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

Submodule cereal updated: a4db5e79e4...fbd45de6e6

2
selfdrive/locationd/locationd.cc
View File

@@ -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)));

2
selfdrive/locationd/ublox_msg.cc
View File

@@ -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);

5
selfdrive/sensord/rawgps/rawgpsd.py
View File

@@ -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"]