diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 9608f4003..e156af5d6 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -20,6 +20,11 @@ const double VALID_POS_STD = 50.0; // m const double MAX_RESET_TRACKER = 5.0; const double SANE_GPS_UNCERTAINTY = 1500.0; // m +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_LOCATION_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095; // s + static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); for (int i = 0; i < floatlist.size(); i++) { @@ -257,7 +262,7 @@ 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) { +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); @@ -265,13 +270,15 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R 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){ + if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { this->determine_gps_mode(current_time); return; } + + double sensor_time = current_time - sensor_time_offset; // Process message - this->last_gps_fix = current_time; + this->last_gps_fix = sensor_time; this->gps_mode = true; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -300,14 +307,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R 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(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + 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->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); + 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_car_state(double current_time, const cereal::CarState::Reader& log) { @@ -443,9 +450,9 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation()); + this->handle_gps(t, log.getGpsLocation(), GPS_LOCATION_SENSOR_TIME_OFFSET); } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal()); + this->handle_gps(t, log.getGpsLocationExternal(), GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -497,9 +504,8 @@ int Localizer::locationd_thread() { } else { gps_location_socket = "gpsLocation"; } - const std::initializer_list service_list = {gps_location_socket, - "cameraOdometry", "liveCalibration", "carState", "carParams", - "accelerometer", "gyroscope"}; + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + "carState", "carParams", "accelerometer", "gyroscope"}; PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index b17ab04b2..280296b06 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -46,7 +46,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); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); void handle_car_state(double current_time, const cereal::CarState::Reader& log); void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 5ff0f2699..f8c03365e 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/selfdrive/locationd/models/live_kf.cc @@ -44,7 +44,7 @@ LiveKalman::LiveKalman() { // init filter this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), - std::vector{3}, std::vector(), 0.2); + std::vector{3}, std::vector(), 0.8); } void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) { diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index e32861cfa..756953021 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -53,6 +53,7 @@ class TestLocationdProc(unittest.TestCase): msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] msg.gpsLocationExternal.latitude = self.lat msg.gpsLocationExternal.longitude = self.lon + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 msg.gpsLocationExternal.altitude = self.alt elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4577d3cdd..e01fe2ac3 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -27022484e3f4c0265ee7243154659b2697de3af7 \ No newline at end of file +14bc91c326f75ce48337720668a1744184c46994 \ No newline at end of file