locationd: initial value for gps_std_factor (#28533)

* setup_gps method

* Add LocalizerGnssSource

* slight refactor

* make it more readable

* Move gnss_source initialization to configure_gnss_souce

* Add gps_variance_factor, gps_vertical_variance_factor and gps_time_offset

* add header changes

selfdrive/locationd/liblocationd.cc

@@ -4,7 +4,7 @@ extern "C" {
   typedef Localizer* Localizer_t;
 
   Localizer *localizer_init(bool has_ublox) {
-    return new Localizer(has_ublox);
+    return new Localizer(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM);
   }
 
   void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,

selfdrive/locationd/locationd.cc

@@ -70,7 +70,7 @@ static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in)
   return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
 }
 
-Localizer::Localizer() {
+Localizer::Localizer(LocalizerGnssSource gnss_source) {
   this->kf = std::make_unique<LiveKalman>();
   this->reset_kalman();
 
@@ -84,10 +84,9 @@ Localizer::Localizer() {
 
   VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
   this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  this->configure_gnss_source(gnss_source);
 }
 
-Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; }
-
 void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd predicted_state = this->kf->get_x();
   MatrixXdr predicted_cov = this->kf->get_P();
@@ -325,12 +324,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
   VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  float ecef_pos_std;
+  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
-  if (ublox_available) {
-    ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2));
-  } else {
-    ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2));
-  }
   MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
   MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
 
@@ -372,10 +366,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
   }
 
   double sensor_time = log.getMeasTime() * 1e-9;
-  if (ublox_available)
+  sensor_time -= this->gps_time_offset;
-    sensor_time -= GPS_UBLOX_SENSOR_TIME_OFFSET;
-  else
-    sensor_time -= GPS_QUECTEL_SENSOR_TIME_OFFSET;
 
   auto ecef_pos_v = log.getPositionECEF().getValue();
   VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
@@ -664,16 +655,33 @@ void Localizer::determine_gps_mode(double current_time) {
   }
 }
 
+void Localizer::configure_gnss_source(LocalizerGnssSource source) {
+  this->gnss_source = source;
+  if (source == LocalizerGnssSource::UBLOX) {
+    this->gps_std_factor = 10.0;
+    this->gps_variance_factor = 1.0;
+    this->gps_vertical_variance_factor = 1.0;
+    this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
+  } else {
+    this->gps_std_factor = 2.0;
+    this->gps_variance_factor = 0.0;
+    this->gps_vertical_variance_factor = 3.0;
+    this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
+  }
+}
+
 int Localizer::locationd_thread() {
-  ublox_available = Params().getBool("UbloxAvailable", true);
+  LocalizerGnssSource source;
   const char* gps_location_socket;
-  if (ublox_available) {
+  if (Params().getBool("UbloxAvailable", true)) {
+    source = LocalizerGnssSource::UBLOX;
     gps_location_socket = "gpsLocationExternal";
-    this->gps_std_factor = 10.0;
   } else {
+    source = LocalizerGnssSource::QCOM;
     gps_location_socket = "gpsLocation";
-    this->gps_std_factor = 2.0;
   }
+
+  this->configure_gnss_source(source);
   const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
                                                           "carState", "carParams", "accelerometer", "gyroscope"};
 

selfdrive/locationd/locationd.h

@@ -21,10 +21,13 @@
 
 #define POSENET_STD_HIST_HALF 20
 
+enum LocalizerGnssSource {
+  UBLOX, QCOM
+};
+
 class Localizer {
 public:
-  Localizer();
+  Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
-  Localizer(bool has_ublox);
 
   int locationd_thread();
 
@@ -81,10 +84,15 @@ private:
   double first_valid_log_time = NAN;
   double ttff = NAN;
   double last_gps_msg = 0;
-  bool ublox_available = true;
+  LocalizerGnssSource gnss_source;
   bool observation_timings_invalid = false;
   std::map<std::string, double> observation_values_invalid;
   bool standstill = true;
   int32_t orientation_reset_count = 0;
   float gps_std_factor;
+  float gps_variance_factor;
+  float gps_vertical_variance_factor;
+  double gps_time_offset;
+
+  void configure_gnss_source(LocalizerGnssSource source);
 };