locationd: log time to first fix (#27990)

* locationd ttff

* log time to first fix

* rename

* max it like laikad

* Update selfdrive/locationd/locationd.cc

* Update ref_commit

* Log when gpsOK first becomes true

* don't forget to update current time!

* stash

* make it deterministic (no proc replay cur time)

* Update ref_commit

* rename to make this clear
old-commit-hash: 03eb02906d9f43799fc93f71ef32331bcf090157

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<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());

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;

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-28ac0bf6aa9142446917301071cf03797a724bf2
+627aa0f54e377d1f3954c58e37c0a15b555e20b3