From 6dba364556bb5eb9cc516bd3d408f1ed16a34bcb Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 4 Sep 2024 23:03:01 +0800 Subject: [PATCH] locationd: remove obsolete locationd.h (#33464) remove obsolete locationd.h --- selfdrive/locationd/.gitignore | 1 - selfdrive/locationd/locationd.h | 99 --------------------------------- 2 files changed, 100 deletions(-) delete mode 100644 selfdrive/locationd/locationd.h diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 11b9f127b2..1a8c72388a 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,3 +1,2 @@ params_learner paramsd -locationd diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h deleted file mode 100644 index f59b7d4bda..0000000000 --- a/selfdrive/locationd/locationd.h +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/transformations/coordinates.hpp" -#include "common/transformations/orientation.hpp" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -#include "system/sensord/sensors/constants.h" -#define VISION_DECIMATION 2 -#define SENSOR_DECIMATION 10 -#include "selfdrive/locationd/models/live_kf.h" - -#define POSENET_STD_HIST_HALF 20 - -enum LocalizerGnssSource { - UBLOX, QCOM -}; - -class Localizer { -public: - Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); - - int locationd_thread(); - - void reset_kalman(double current_time = NAN); - void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); - void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); - void finite_check(double current_time = NAN); - void time_check(double current_time = NAN); - void update_reset_tracker(); - bool is_gps_ok(); - bool critical_services_valid(const std::map &critical_services); - bool is_timestamp_valid(double current_time); - void determine_gps_mode(double current_time); - bool are_inputs_ok(); - void observation_timings_invalid_reset(); - - void build_pose_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid); - void build_live_pose(cereal::LivePose::Builder& fix); - - Eigen::VectorXd get_position_geodetic(); - Eigen::VectorXd get_state(); - Eigen::VectorXd get_stdev(); - - 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); - void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); - - void input_fake_gps_observations(double current_time); - -private: - std::unique_ptr kf; - - Eigen::VectorXd calib; - MatrixXdr device_from_calib; - MatrixXdr calib_from_device; - bool calibrated = false; - - double car_speed = 0.0; - double last_reset_time = NAN; - std::deque posenet_stds; - - std::unique_ptr converter; - - int64_t unix_timestamp_millis = 0; - double reset_tracker = 0.0; - bool gps_mode = false; - 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; - Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std - - void configure_gnss_source(const LocalizerGnssSource &source); -};