diff --git a/CHANGELOGS.md b/CHANGELOGS.md index ea8167dcad..8c44e7194e 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -48,6 +48,10 @@ sunnypilot - 0.9.5.1 (2023-10-xx) * FIXED: Retain hotspot/tethering state was not consistently saved * FIXED: Map stuck in "Map Loading" if comma Prime is active * FIXED: UI elements adjustments +* FIXED: OpenStreetMap implementation on C3X devices + * M-TSC + * Altitude (ALT.) display on Developer UI + * Current street name on top of driving screen when "OSM Debug UI" is enabled * Kia Seltos Non-SCC 2023-24 support thanks to Moodkiller and jeroid_! sunnypilot - 0.9.4.1 (2023-08-11) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 968ad310d9..d4efcbad64 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import json +import math import threading from traceback import print_exception import numpy as np @@ -36,6 +37,16 @@ def excepthook(args): threading.excepthook = excepthook +class GPSData: + def __init__(self, unixTimestampMillis, latitude, longitude, speed, bearingDeg, accuracy, bearingAccuracyDeg): + self.unixTimestampMillis = unixTimestampMillis + self.latitude = latitude + self.longitude = longitude + self.speed = speed + self.bearingDeg = bearingDeg + self.accuracy = accuracy + self.bearingAccuracyDeg = bearingAccuracyDeg + class MapD(): def __init__(self): @@ -80,27 +91,37 @@ class MapD(): self._op_enabled = hud_control.speedVisible def update_gps(self, sm): - sock = 'gpsLocationExternal' + sock = 'liveLocationKalman' if not sm.updated[sock] or not sm.valid[sock]: return log = sm[sock] - self.last_gps = log + # ignore the message if the fix is invalid - if log.flags % 2 == 0: + if not log.positionGeodetic.valid: return self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970. - self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float)) - self.location_deg = (log.latitude, log.longitude) - self.bearing_rad = np.radians(log.bearingDeg, dtype=float) - self.gps_speed = log.speed - self.location_stdev = log.accuracy # log accuracies are presumably 1 standard deviation. + self.location_rad = np.radians(np.array([log.positionGeodetic.value[0], log.positionGeodetic.value[1]], dtype=float)) + self.location_deg = (log.positionGeodetic.value[0], log.positionGeodetic.value[1]) + self.bearing_rad = math.degrees(log.calibratedOrientationNED.value[2]) + self.gps_speed = log.velocityCalibrated.value[2] + self.location_stdev = 1 # hardcoding, this is not available in liveLocationKalman + + self.last_gps = GPSData( + unixTimestampMillis=self.last_gps_fix_timestamp, + latitude=self.location_deg[0], + longitude=self.location_deg[1], + speed=self.gps_speed, + bearingDeg=self.bearing_rad, + accuracy=self.location_stdev, # assuming this is equivalent information + bearingAccuracyDeg=1. # you'll need to assign this if available + ) _debug('Mapd: ********* Got GPS fix' + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n' - + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n' + + f'Bearing: {log.calibratedOrientationNED.value[2]} +/- {1. * 2.} deg.\n' + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' + '*******', log_to_cloud=False) @@ -221,8 +242,8 @@ class MapD(): current_road_name = self.route.current_road_name map_data_msg = messaging.new_message('liveMapDataSP') - map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \ - sm.all_valid(service_list=['gpsLocationExternal']) + map_data_msg.valid = sm.all_alive(service_list=['liveLocationKalman']) and \ + sm.all_valid(service_list=['liveLocationKalman']) liveMapDataSP = map_data_msg.liveMapDataSP liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis @@ -271,7 +292,7 @@ def mapd_thread(sm=None, pm=None): # *** setup messaging if sm is None: - sm = messaging.SubMaster(['gpsLocationExternal', 'carControl']) + sm = messaging.SubMaster(['liveLocationKalman', 'carControl']) if pm is None: pm = messaging.PubMaster(['liveMapDataSP']) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 927991b841..b7bc0426cd 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -522,7 +522,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { const auto nav_instruction = sm["navInstruction"].getNavInstruction(); const auto car_control = sm["carControl"].getCarControl(); const auto radar_state = sm["radarState"].getRadarState(); - const auto gpsLocationExternal = sm["gpsLocationExternal"].getGpsLocationExternal(); const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto lateral_plan_sp = sm["lateralPlanSP"].getLateralPlanSP(); @@ -572,6 +571,29 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { splitPanelVisible = s.scene.map_visible || s.scene.onroad_settings_visible; // ############################## DEV UI START ############################## + + if (sm.updated("liveLocationKalman")) { + auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); + auto locationd_pos = locationd_location.getPositionGeodetic(); + auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); + auto locationd_velocity = locationd_location.getVelocityCalibrated(); + + // 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; + + auto locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough); + + if (locationd_valid) { + gpsAccuracy = 1.0; //Hardcoding to 1 because not available in liveLocationKalman + altitude = locationd_pos.getValue()[2]; + + bearingAccuracyDeg = 1.0; //Hardcoding to 1 because not available in liveLocationKalman + bearingDeg = locationd_orientation.getValue()[2]; + + } + } + lead_d_rel = radar_state.getLeadOne().getDRel(); lead_v_rel = radar_state.getLeadOne().getVRel(); lead_status = radar_state.getLeadOne().getStatus(); @@ -583,13 +605,9 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { memoryUsagePercent = sm["deviceState"].getDeviceState().getMemoryUsagePercent(); devUiEnabled = s.scene.dev_ui_enabled; devUiInfo = s.scene.dev_ui_info; - gpsAccuracy = gpsLocationExternal.getAccuracy(); - altitude = gpsLocationExternal.getAltitude(); vEgo = car_state.getVEgo(); aEgo = car_state.getAEgo(); steeringTorqueEps = car_state.getSteeringTorqueEps(); - bearingAccuracyDeg = gpsLocationExternal.getBearingAccuracyDeg(); - bearingDeg = gpsLocationExternal.getBearingDeg(); torquedUseParams = (ltp.getUseParams() || s.scene.live_torque_toggle) && !s.scene.torqued_override; latAccelFactorFiltered = ltp.getLatAccelFactorFiltered(); frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();