openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

99 lines
3.3 KiB

#pragma once
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <memory>
#include <map>
#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 "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, 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 is_gps_ok();
bool critical_services_valid(std::map<std::string, double> 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();
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);
Sensor events splitup (#25714) * 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: 29d3ed2ce63a65f793dc0ecb553180a0d0fba03e
3 years ago
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<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 reset_tracker = 0.0;
bool device_fell = false;
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;
Laikad: set active (#26850) * laikad update, renaming * update locationd * fix naming * address PR comments * upsi * . * draft to fix replay * fix process relay to allow no response for messages * final fix for process replay * . * bump cereal * update process replay ref commit * reduce wait time * . * last ref change * move laikad helpers to laika * . * fix ublox test * update refs * add proper qcom replay support * fix gnss support if both is available * update refs * remove left over * revert laikad msg * move laika back to master * init * fix gps valid flag * change time * add gnss to ignore * remove gps_valid flag * . * adopt orientation reset threshold * . * update laikad * . * fix stanstill KF resets * test orienation reset count * update laika * bump cereal * fix process replay * update laika repo * remove handle gps * add extra logging for cache * . * add more log * . * . * update laika * dont remove gps code * inc min satellite count * update magic vals and add acc drop * update laika * upsi * rem * bump laika * use nav and correct * more fixes * use sftp * No more glonass * Revert "No more glonass" This reverts commit a76124da50a1e25f423ad1137c7a046e1d57811d. * nump laika * back support old ephemeris cache * add health to ephemeris message * bump laika * remove print * fix laikad tests * clean * remove extra log * bump laika * inc timeout for plotjuggler build * rem cache clear * . * enable gps after checks Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com> Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com> Co-authored-by: Shane Smiskol <shane@smiskol.com> old-commit-hash: 88423e25df8bfbd8e6a3275eb086f468cf9a59f1
2 years ago
std::map<std::string, double> 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(LocalizerGnssSource source);
};