diff --git a/cereal b/cereal index f6afcaf723..6ac88564fc 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f6afcaf723bbed3c6c1d6dca69b451b4c87e2ef1 +Subproject commit 6ac88564fcf4893c400295e5bc1ff46649073ed6 diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index de2373c1da..6c0307bd64 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -180,6 +180,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); fix.setDeviceStable(!this->device_fell); fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); + fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); this->device_fell = false; //fix.setGpsWeek(this->time.week); @@ -529,6 +530,9 @@ void Localizer::time_check(double current_time) { if (std::isnan(this->last_reset_time)) { this->last_reset_time = current_time; } + if (std::isnan(this->first_valid_log_time)) { + this->first_valid_log_time = current_time; + } double filter_time = this->kf->get_filter_time(); bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); if (big_time_gap) { @@ -700,6 +704,11 @@ int Localizer::locationd_thread() { bool gpsOK = this->is_gps_ok(); bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); + // Log time to first fix + if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { + this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); + } + MessageBuilder msg_builder; kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); pm.send("liveLocationKalman", bytes.begin(), bytes.size()); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 6366b84f8e..e2b2096afc 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -78,6 +78,8 @@ private: double reset_tracker = 0.0; bool device_fell = false; bool gps_mode = false; + double first_valid_log_time = NAN; + double ttff = NAN; double last_gps_msg = 0; bool ublox_available = true; bool observation_timings_invalid = false; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1c7c97b60a..2e1c439698 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -28ac0bf6aa9142446917301071cf03797a724bf2 +627aa0f54e377d1f3954c58e37c0a15b555e20b3