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

Merge branch 'devel-staging' of https://github.com/commaai/openpilot into devel

Browse Source
This commit is contained in:
dragonpilot
2020-02-05 13:46:07 +10:00
parent 50ecefc689 43c07122e2
commit 87a694ebcb
217 changed files with 50402 additions and 95831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+4 -4
View File
@@ -38,8 +38,8 @@ def is_calibration_valid(vp):
def sanity_clip(vp):
if np.isnan(vp).any():
vp = VP_INIT
return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)]
return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)])
def intrinsics_from_vp(vp):
@@ -96,6 +96,7 @@ class Calibrator():
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
new_vp = sanity_clip(new_vp)
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
@@ -103,8 +104,7 @@ class Calibrator():
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
self.vp = sanity_clip(raw_vp)
self.vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
self.update_status()
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
selfdrive/locationd/locationd_yawrate.cc
+19 -26
View File
@@ -10,7 +10,7 @@
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
if (dt < 0) {
@@ -24,7 +24,7 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
double y = meas - C * x;
double S = R + C * P * C.transpose();
Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
x = x + K * y;
P = (I - K * C) * P;
}
@@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
}
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
double R = pow(5 * camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
@@ -57,34 +57,27 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
Localizer::Localizer() {
// States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
// States: [yaw rate, gyro bias]
A <<
1, 1, 0, 0,
0, 1, 0, 0,
0, 0, 1, 1,
0, 0, 0, 1;
I <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
1, 0,
0, 1;
Q <<
0, 0, 0, 0,
0, pow(0.1, 2.0), 0, 0,
0, 0, 0, 0,
0, 0, pow(0.005 / 100.0, 2.0), 0;
pow(.1, 2.0), 0,
0, pow(0.05/ 100.0, 2.0),
P <<
pow(100.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0,
0, 0, pow(100.0, 2.0), 0,
0, 0, 0, pow(100.0, 2.0);
pow(10000.0, 2.0), 0,
0, pow(10000.0, 2.0);
C_posenet << 1, 0, 0, 0;
C_gyro << 1, 0, 1, 0;
x << 0, 0, 0, 0;
I <<
1, 0,
0, 1;
R_gyro = pow(0.25, 2.0);
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
R_gyro = pow(0.025, 2.0);
}
void Localizer::handle_log(cereal::Event::Reader event) {
@@ -133,7 +126,7 @@ extern "C" {
}
double localizer_get_bias(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x[2];
return loc->x[1];
}
double * localizer_get_state(void * localizer) {
selfdrive/locationd/locationd_yawrate.h
+8 -8
View File
@@ -7,22 +7,22 @@
class Localizer
{
Eigen::Matrix4d A;
Eigen::Matrix4d I;
Eigen::Matrix4d Q;
Eigen::Matrix<double, 1, 4> C_posenet;
Eigen::Matrix<double, 1, 4> C_gyro;
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
public:
Eigen::Vector4d x;
Eigen::Matrix4d P;
Eigen::Vector2d x;
Eigen::Matrix2d P;
double steering_angle = 0;
double car_speed = 0;
double posenet_speed = 0;
selfdrive/locationd/paramsd.cc
+2 -1
View File
@@ -20,6 +20,7 @@
#include "locationd_yawrate.h"
#include "params_learner.h"
#include "common/util.h"
void sigpipe_handler(int sig) {
LOGE("SIGPIPE received");
@@ -153,7 +154,7 @@ int main(int argc, char *argv[]) {
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[2]);
live_params.setGyroBias(localizer.x[1]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
selfdrive/locationd/ubloxd_main.cc
+1
View File
@@ -19,6 +19,7 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/util.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"