mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-04 00:59:34 +08:00
Kurt Nistelberger
fb06d2769f
* PoC of reading sensors via interrupts instead of polling
* add Gyro and draft for magn
* add more functionality to gpio.cc
* change LSM gyro to interrupt
* resolve rebase conflict
* update BMX accel interrupt impl
* add interrupt collector thread to fetch in parallel
* change get_event interface to return true on successful read
* update BMX gyro interrupt impl
* update gpio.h/.cc according to comments
* address comments, rename Edgetype enum
* Edgetype to EdgeType
* update sensor interrupt interface
* add error handling, and read fd on trigger
* avoid sending empty messages
* fix build
* use gpiochip
* less diff
* gpiochip on both edges, but skip falling edge if rising edge is detected
* init last_ts with 0
* update sensord testcases
* update sensord testsweet
* test for pipeline
* readd with_process
* add null check
* move tests update to seperate PR
* sensord: improve test coverage (#25683)
* update sensord-interrupt testsweet
* address review comments
* inc stddev threshold
* fix format string
* add version 0 check again
* relax strictness after c3 with bmx tests
* relax strictness after tests
Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
* address PR comments
* fix typo
* remove 4ms limit, and skip first 0.5sec of data
* revert disable_interuppt change to destructor
* fix and remove timing skip
* make gpiochip generic
* sensord port
* change from sensorEvents to separated events
* fix gyro usage
* add splitted sensor tests
* modify debug script sensor_data_to_hist.py
* refactor get_event interface to remove sensorEvent message type
* update locationd to non sensorEvent usage
* tmp commit
* fix replay
* fix accelerometer type
* fix sensor to hist debug script
* update sensord tests to split events
* remove rebase artifacts
* port test_sensord.py
* small clean up
* change cereal to sensorEvents-splitup branch
* upate sensorEvents in regen
* fix route generation for splitted sensor events
* regen cleanUp from sensorEvents change
* .
* remove light and temp from locationd
* add generic init delay per sensor
* .
* update routes
* move bmx gyro/accel to its own channel
* adopt sensor tests to bmx channel
* remove rebase artifacts
* fix sensord test
* handle bmx not present
* add bmx sockets to regen
* .
* .
* code cleanUp
* .
* address PR comments
* address PR comments
* address PR comments
* lsm clean up
* readd sensorEvents
* rever regen.py
* .
* update replay refs
* move channels
* fix artifact
* bump cereal
* update refs
* fix timing issue
Co-authored-by: Bruce Wayne <batman@workstation-eu-intern2.eu.local>
Co-authored-by: gast04 <kurt.nistelberger@gmail.com>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 29d3ed2ce6
76 lines
2.4 KiB
C++
Executable File
76 lines
2.4 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 "common/params.h"
|
|
#include "common/swaglog.h"
|
|
#include "common/timing.h"
|
|
#include "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, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
|
|
void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
|
|
void finite_check(double current_time = NAN);
|
|
void time_check(double current_time = NAN);
|
|
void update_reset_tracker();
|
|
bool isGpsOK();
|
|
void determine_gps_mode(double current_time);
|
|
|
|
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
|
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
|
void build_live_location(cereal::LiveLocationKalman::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);
|
|
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<LiveKalman> 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<double> posenet_stds;
|
|
|
|
std::unique_ptr<LocalCoord> converter;
|
|
|
|
int64_t unix_timestamp_millis = 0;
|
|
double last_gps_fix = 0;
|
|
double reset_tracker = 0.0;
|
|
bool device_fell = false;
|
|
bool gps_mode = false;
|
|
};
|