mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-14 19:43:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.8.6 release

Browse Source
This commit is contained in:
Vehicle Researcher
2021-07-14 17:34:36 -07:00
parent ce6564c684
commit b201dc1996
429 changed files with 9828 additions and 11930 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+14 -12
View File
@@ -1,6 +1,8 @@
#include <sys/time.h>
#include <sys/resource.h>
#include <cmath>
#include "locationd.h"
using namespace EKFS;
@@ -188,20 +190,20 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
const cereal::SensorEventData::Reader& sensor_reading = log[i];
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (sensor_reading.getTimestamp() == 0){
if (sensor_reading.getTimestamp() == 0) {
continue;
}
double sensor_time = 1e-9 * sensor_reading.getTimestamp();
// sensor time and log time should be close
if (abs(current_time - sensor_time) > 0.1) {
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
return;
}
// TODO: handle messages from two IMUs at the same time
if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) {
if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
continue;
}
@@ -245,7 +247,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
return;
}
if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK){
if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) {
return;
}
@@ -279,7 +281,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 50.0) {
} 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);
}
@@ -306,7 +308,7 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)){
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
return;
}
@@ -332,7 +334,7 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (log.getRpyCalib().size() > 0) {
auto calib = floatlist2vector(log.getRpyCalib());
if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)){
if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
return;
}
@@ -350,19 +352,19 @@ void Localizer::reset_kalman(double current_time) {
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite){
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::time_check(double current_time) {
if (isnan(this->last_reset_time)) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap){
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
@@ -467,7 +469,7 @@ int Localizer::locationd_thread() {
}
int main() {
setpriority(PRIO_PROCESS, 0, -20);
set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();