spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

nav: use calibrated orientation (#21853) 

* calibrate map

* msg added
pull/21859/head
HaraldSchafer 2021-08-04 09:58:04 -07:00 committed by GitHub
parent fefc70b4ca
commit 4b209f31a1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/locationd/locationd.cc
View File

@ -114,6 +114,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
@ -137,6 +138,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
658824c1198bd9e4c2b2c6d1c9de9bcae04c7057
db2abbe672c77bb7fbf904f195174cbfe9fc6f27

2
selfdrive/ui/qt/maps/map.cc
View File

@ -129,7 +129,7 @@ void MapWindow::timerUpdate() {
if (localizer_valid) {
auto pos = location.getPositionGeodetic();
auto orientation = location.getOrientationNED();
auto orientation = location.getCalibratedOrientationNED();
float velocity = location.getVelocityCalibrated().getValue()[0];
float bearing = RAD2DEG(orientation.getValue()[2]);