diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 47c6c150b4..e2b85d93a1 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -4,7 +4,7 @@ extern "C" { typedef Localizer* Localizer_t; Localizer *localizer_init(bool has_ublox) { - return new Localizer(); + return new Localizer(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM); } void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index c22230a049..401de0cfdd 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -15,6 +15,7 @@ const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 const double ROTATION_SANITY_CHECK = 10.0; // rad/s const double TRANS_SANITY_CHECK = 200.0; // m/s const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m @@ -24,11 +25,15 @@ const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker const double DECAY = 0.99995; // same as reset tracker const double MAX_FILTER_REWIND_TIME = 0.8; // s +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s const float GPS_POS_STD_THRESHOLD = 50.0; const float GPS_VEL_STD_THRESHOLD = 5.0; const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; -const float GPS_POS_STD_RESET_THRESHOLD = 10.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 1.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; @@ -67,7 +72,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(); this->reset_kalman(); @@ -81,6 +86,7 @@ Localizer::Localizer() { VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + this->configure_gnss_source(gnss_source); } void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { @@ -293,6 +299,64 @@ void Localizer::input_fake_gps_observations(double current_time) { this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { + // ignore the message if the fix is invalid + bool gps_invalid_flag = (log.getFlags() % 2 == 0); + bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); + bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); + bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); + + if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + //this->gps_valid = false; + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = current_time - sensor_time_offset; + + // Process message + //this->gps_valid = true; + this->gps_mode = true; + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(geodetic); + + 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 = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * 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(); + + 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))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { + LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + } else if (gps_est_error > 100.0) { + LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } + + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { @@ -301,22 +365,21 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: } double sensor_time = log.getMeasTime() * 1e-9; + sensor_time -= this->gps_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]); - auto ecef_pos_std = log.getPositionECEF().getStd(); - VectorXd ecef_pos_var = Vector3d(ecef_pos_std[0], ecef_pos_std[1], ecef_pos_std[2]).array().square().matrix(); - double ecef_pos_std_norm = std::sqrt(ecef_pos_var.sum()); - MatrixXdr ecef_pos_R = ecef_pos_var.asDiagonal(); + // indexed at 0 cause all std values are the same MAE + auto ecef_pos_std = log.getPositionECEF().getStd()[0]; + MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); auto ecef_vel_v = log.getVelocityECEF().getValue(); VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - auto ecef_vel_std = log.getVelocityECEF().getStd(); - VectorXd ecef_vel_var = Vector3d(ecef_vel_std[0], ecef_vel_std[1], ecef_vel_std[2]).array().square().matrix(); - double ecef_vel_std_norm = std::sqrt(ecef_vel_var.sum()); - MatrixXdr ecef_vel_R = ecef_vel_var.asDiagonal(); + // indexed at 0 cause all std values are the same MAE + auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; + MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -339,13 +402,13 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: } VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); - if (ecef_pos_std_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) { + if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { this->determine_gps_mode(current_time); return; } // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD; + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; orientation_reset &= !this->standstill; if (orientation_reset) { @@ -354,7 +417,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: this->orientation_reset_count = 0; } - if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std_norm < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { // always reset on first gps message and if the location is off but the accuracy is high LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); @@ -524,8 +587,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->handle_sensor(t, log.getAccelerometer()); } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); - } else if (log.isGnssMeasurements()) { - this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -586,12 +653,38 @@ void Localizer::determine_gps_mode(double current_time) { } } +void Localizer::configure_gnss_source(const 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() { - const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", + LocalizerGnssSource source; + const char* gps_location_socket; + if (Params().getBool("UbloxAvailable")) { + source = LocalizerGnssSource::UBLOX; + gps_location_socket = "gpsLocationExternal"; + } else { + source = LocalizerGnssSource::QCOM; + gps_location_socket = "gpsLocation"; + } + + this->configure_gnss_source(source); + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 615251668d..f3069048cd 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -22,10 +22,13 @@ #define POSENET_STD_HIST_HALF 20 +enum LocalizerGnssSource { + UBLOX, QCOM +}; class Localizer { public: - Localizer(); + Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); int locationd_thread(); @@ -53,6 +56,7 @@ public: void handle_msg_bytes(const char *data, const size_t size); void handle_msg(const cereal::Event::Reader& log); void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); void handle_car_state(double current_time, const cereal::CarState::Reader& log); void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); @@ -81,8 +85,15 @@ private: double first_valid_log_time = NAN; double ttff = NAN; double last_gps_msg = 0; + LocalizerGnssSource gnss_source; bool observation_timings_invalid = false; std::map 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(const LocalizerGnssSource &source); }; diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 1fb36bf2fa..6c6ac33431 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -14,7 +14,7 @@ from openpilot.selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): - LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -35,14 +35,25 @@ class TestLocationdProc(unittest.TestCase): msg = messaging.new_message(name) except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - if name == "gnssMeasurements": - msg.gnssMeasurements.measTime = t - msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] - msg.gnssMeasurements.positionECEF.std = [0,0,0] - msg.gnssMeasurements.positionECEF.valid = True - msg.gnssMeasurements.velocityECEF.value = [] - msg.gnssMeasurements.velocityECEF.std = [0,0,0] - msg.gnssMeasurements.velocityECEF.valid = True + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index c0a49e3324..9256b6015c 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -433,6 +433,13 @@ def laikad_config_pubsub_callback(params, cfg, lr): cfg.main_pub_drained = True +def locationd_config_pubsub_callback(params, cfg, lr): + ublox = params.get_bool("UbloxAvailable") + sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) + + cfg.pubs = set(cfg.pubs) - sub_keys + + CONFIGS = [ ProcessConfig( proc_name="controlsd", @@ -487,11 +494,12 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements", - "liveCalibration", "carState", "carParams" + "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", + "liveCalibration", "carState", "carParams", "gpsLocation" ], subs=["liveLocationKalman"], ignore=["logMonoTime", "valid"], + config_callback=locationd_config_pubsub_callback, tolerance=NUMPY_TOLERANCE, ), ProcessConfig( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 955c5c7e94..cee528c62d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3fd85c41e61044a86bf364b9a7a0470e5758f231 +20022e4a1b5f5df343465180a3865b0c78890544 \ No newline at end of file