mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 02:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Nav: show gps as long as it's 100m accuracy (#28713)

Browse Source 
* Nav: show gps as long as it's 100m accuracy

* Get norm

* get std
old-commit-hash: 85c4b1a400e9ab80536c0bf2ac8f1bacc4476c2c
This commit is contained in:
Harald Schäfer
2023-06-27 19:09:45 -07:00
committed by GitHub
parent b6679b50ee
commit 659fc99be4
2 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/qt/maps/map.cc
+7 -4
View File
@@ -136,8 +136,11 @@ void MapWindow::updateState(const UIState &s) {
auto locationd_orientation = locationd_location.getCalibratedOrientationNED();
auto locationd_velocity = locationd_location.getVelocityCalibrated();
locationd_valid = (locationd_location.getStatus() == cereal::LiveLocationKalman::Status::VALID) &&
locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid();
// Check std norm
auto pos_ecef_std = locationd_location.getPositionECEF().getStd();
bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100;
locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough);
if (locationd_valid) {
last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]);
@@ -168,7 +171,7 @@ void MapWindow::updateState(const UIState &s) {
initLayers();
if (locationd_valid || laikad_valid) {
if (locationd_valid) {
map_instructions->noError();
// Update current location marker
@@ -201,7 +204,7 @@ void MapWindow::updateState(const UIState &s) {
auto i = sm["navInstruction"].getNavInstruction();
emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining());
if (locationd_valid || laikad_valid) {
if (locationd_valid) {
m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged
emit instructionsChanged(i);
selfdrive/ui/qt/maps/map.h
-1
View File
@@ -106,7 +106,6 @@ private:
std::optional<QMapbox::Coordinate> last_position;
std::optional<float> last_bearing;
FirstOrderFilter velocity_filter;
bool laikad_valid = false;
bool locationd_valid = false;
MapInstructions* map_instructions;