diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index e2b85d93a1..47c6c150b4 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(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM); + return new Localizer(); } 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 401de0cfdd..c22230a049 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -15,7 +15,6 @@ 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 @@ -25,15 +24,11 @@ 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 = 2.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_POS_STD_RESET_THRESHOLD = 10.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 1.0; const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; @@ -72,7 +67,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(LocalizerGnssSource gnss_source) { +Localizer::Localizer() { this->kf = std::make_unique(); this->reset_kalman(); @@ -86,7 +81,6 @@ Localizer::Localizer(LocalizerGnssSource gnss_source) { 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) { @@ -299,64 +293,6 @@ 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()) { @@ -365,21 +301,22 @@ 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]); - // 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_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(); auto ecef_vel_v = log.getVelocityECEF().getValue(); VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - // 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(); + 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(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -402,13 +339,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 > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + if (ecef_pos_std_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) { this->determine_gps_mode(current_time); return; } // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD; orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; orientation_reset &= !this->standstill; if (orientation_reset) { @@ -417,7 +354,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 < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std_norm < 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); @@ -587,12 +524,8 @@ 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.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.isGnssMeasurements()) { + this->handle_gnss(t, log.getGnssMeasurements()); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -653,38 +586,12 @@ 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() { - 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", + const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index f3069048cd..615251668d 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -22,13 +22,10 @@ #define POSENET_STD_HIST_HALF 20 -enum LocalizerGnssSource { - UBLOX, QCOM -}; class Localizer { public: - Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); + Localizer(); int locationd_thread(); @@ -56,7 +53,6 @@ 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); @@ -85,15 +81,8 @@ 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 6c6ac33431..1fb36bf2fa 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 = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -35,25 +35,14 @@ class TestLocationdProc(unittest.TestCase): msg = messaging.new_message(name) except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - - 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 + 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 9256b6015c..c0a49e3324 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -433,13 +433,6 @@ 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", @@ -494,12 +487,11 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", - "liveCalibration", "carState", "carParams", "gpsLocation" + "cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements", + "liveCalibration", "carState", "carParams" ], 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 51c84c4bdd..d5ff4b6069 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4846f6f416128ba74837829643aa3fe0897c5621 \ No newline at end of file +9d6369c5aa88c37499c330a06aa7a81a5f4f3283 \ No newline at end of file