mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-21 16:23:50 +08:00
Dean Lee
7222d0f20d
* cleanup include path * continue * format includes * fix testraw.cc * remove include path from SConstruct * regroup * rebase master * almost done * apply review * rename FileReader.xx to filereader.xx * rename Unlogger.x->unlogger.x * rename FrameReader.xx -> framereader.xx * apply reviews * ui.h * continue * fix framebuffer.cc build error:mv util.h up * full path to msm_media_info * fix qcom2 camerad Co-authored-by: Comma Device <device@comma.ai>
64 lines
2.0 KiB
C++
Executable File
64 lines
2.0 KiB
C++
Executable File
#pragma once
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
#include <fstream>
|
|
#include <memory>
|
|
#include <string>
|
|
|
|
#include "cereal/messaging/messaging.h"
|
|
#include "common/transformations/coordinates.hpp"
|
|
#include "common/transformations/orientation.hpp"
|
|
#include "selfdrive/common/params.h"
|
|
#include "selfdrive/common/swaglog.h"
|
|
#include "selfdrive/common/timing.h"
|
|
#include "selfdrive/common/util.h"
|
|
|
|
#include "selfdrive/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
|
|
|
|
class Localizer {
|
|
public:
|
|
Localizer();
|
|
|
|
int locationd_thread();
|
|
|
|
void reset_kalman(double current_time = NAN);
|
|
void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
|
|
void finite_check(double current_time = NAN);
|
|
|
|
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
|
bool inputsOK, bool sensorsOK, bool gpsOK);
|
|
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
|
|
|
Eigen::VectorXd get_position_geodetic();
|
|
|
|
void handle_msg_bytes(const char *data, const size_t size);
|
|
void handle_msg(const cereal::Event::Reader& log);
|
|
void handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log);
|
|
void handle_gps(double current_time, const cereal::GpsLocationData::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);
|
|
|
|
private:
|
|
std::unique_ptr<LiveKalman> kf;
|
|
|
|
Eigen::VectorXd calib;
|
|
MatrixXdr device_from_calib;
|
|
MatrixXdr calib_from_device;
|
|
bool calibrated = false;
|
|
|
|
double car_speed = 0.0;
|
|
std::deque<double> posenet_stds;
|
|
|
|
std::unique_ptr<LocalCoord> converter;
|
|
|
|
int64_t unix_timestamp_millis = 0;
|
|
double last_gps_fix = 0;
|
|
bool device_fell = false;
|
|
};
|