mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-21 22:12:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Only trust locationd for nav (#27579)

Browse Source 
* Only trust locationd for nav

* unused var
This commit is contained in:
Harald Schäfer
2023-03-14 09:25:09 -07:00
committed by GitHub
parent 4692cf7d1c
commit d75ee19def
2 changed files with 1 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/qt/maps/map.cc
-38
View File
@@ -24,8 +24,6 @@ const float MAX_PITCH = 50;
const float MIN_PITCH = 0;
const float MAP_SCALE = 2;
const float VALID_POS_STD = 50.0; // m
const QString ICON_SUFFIX = ".png";
MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) {
@@ -125,42 +123,6 @@ void MapWindow::updateState(const UIState &s) {
}
}
// TODO should check a valid/status flag
if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){
auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements();
auto laikad_pos = laikad_location.getPositionECEF();
auto laikad_pos_ecef = laikad_pos.getValue();
auto laikad_pos_std = laikad_pos.getStd();
auto laikad_velocity_ecef = laikad_location.getVelocityECEF().getValue();
laikad_valid = laikad_pos.getValid() && Eigen::Vector3d(laikad_pos_std[0], laikad_pos_std[1], laikad_pos_std[2]).norm() < VALID_POS_STD;
if (laikad_valid && !locationd_valid) {
ECEF ecef = {.x = laikad_pos_ecef[0], .y = laikad_pos_ecef[1], .z = laikad_pos_ecef[2]};
Geodetic laikad_pos_geodetic = ecef2geodetic(ecef);
last_position = QMapbox::Coordinate(laikad_pos_geodetic.lat, laikad_pos_geodetic.lon);
// Compute NED velocity
LocalCoord converter(ecef);
ECEF next_ecef = {.x = ecef.x + laikad_velocity_ecef[0], .y = ecef.y + laikad_velocity_ecef[1], .z = ecef.z + laikad_velocity_ecef[2]};
Eigen::VectorXd ned_vel = converter.ecef2ned(next_ecef).to_vector() - converter.ecef2ned(ecef).to_vector();
float velocity = ned_vel.norm();
velocity_filter.update(velocity);
// Convert NED velocity to angle
if (velocity > 1.0) {
float new_bearing = fmod(RAD2DEG(atan2(ned_vel[1], ned_vel[0])) + 360.0, 360.0);
if (last_bearing) {
float delta = 0.1 * angle_difference(*last_bearing, new_bearing); // Smooth heading
last_bearing = fmod(*last_bearing + delta + 360.0, 360.0);
} else {
last_bearing = new_bearing;
}
}
}
}
if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) {
qWarning() << "Got new navRoute from navd. Opening map:" << allow_open;
selfdrive/ui/ui.cc
+1 -2
View File
@@ -252,8 +252,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements",
"uiPlan",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan",
});
Params params;