liveLocationKalman, livePose, comfortBrake2.4, korean message (#159)

* fix routeinfo...

* livePose -> liveLocationKalman

* fix..

* for debug.. socketmaster

* fix.. SCC track

* fix.. carrotman

* comfortBrake 2.5 -> 2.4

* fix..

* test gps debug

* test gps

* revert

* test gps msg

* test restart

* ff

* ff

* ff

* liveDelay(SAD), latSmoothSeconds (#158)

* SAD default: 0 (use liveDelay)

cereal/messaging/socketmaster.cc

@@ -186,6 +186,9 @@ SubMaster::~SubMaster() {
 
 PubMaster::PubMaster(const std::vector<const char *> &service_list) {
   for (auto name : service_list) {
+    if (services.count(name) == 0) {
+      printf("PubMaster Error: unknown service '%s'\n", name);
+    }
     assert(services.count(name) > 0);
     PubSocket *socket = PubSocket::create(message_context.context(), name);
     assert(socket);

cereal/services.py

@@ -36,7 +36,7 @@ _services: dict[str, tuple] = {
   "errorLogMessage": (True, 0., 1),
   "liveCalibration": (True, 4., 4),
   "liveTorqueParameters": (True, 4., 1),
-  #"liveDelay": (True, 4., 1),
+  "liveDelay": (True, 4., 1),
   "androidLog": (True, 0.),
   "carState": (True, 100., 10),
   "carControl": (True, 100., 10),
@@ -51,7 +51,8 @@ _services: dict[str, tuple] = {
   "gnssMeasurements": (True, 10., 10),
   "clocks": (True, 0.1, 1),
   "ubloxRaw": (True, 20.),
-  "livePose": (True, 20., 4),
+  #"livePose": (True, 20., 4),
+  "liveLocationKalman": (True, 20., 5),
   "liveParameters": (True, 20., 5),
   "cameraOdometry": (True, 20., 10),
   "thumbnail": (True, 1 / 60., 1),

common/mock/__init__.py

@@ -8,12 +8,12 @@ import functools
 import threading
 from cereal.messaging import PubMaster
 from cereal.services import SERVICE_LIST
-from openpilot.common.mock.generators import generate_livePose
+from openpilot.common.mock.generators import generate_liveLocationKalman
 from openpilot.common.realtime import Ratekeeper
 
 
 MOCK_GENERATOR = {
-  "livePose": generate_livePose
+  "liveLocationKalman": generate_liveLocationKalman
 }
 
 

common/mock/generators.py

@@ -1,14 +1,20 @@
 from cereal import messaging
 
 
-def generate_livePose():
+LOCATION1 = (32.7174, -117.16277)
-  msg = messaging.new_message('livePose')
+LOCATION2 = (32.7558, -117.2037)
-  meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
+
-  msg.livePose.orientationNED = meas
+LLK_DECIMATION = 10
-  msg.livePose.velocityDevice = meas
+RENDER_FRAMES = 15
-  msg.livePose.angularVelocityDevice = meas
+DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
-  msg.livePose.accelerationDevice = meas
+
-  msg.livePose.inputsOK = True
+
-  msg.livePose.posenetOK = True
+def generate_liveLocationKalman(location=LOCATION1):
-  msg.livePose.sensorsOK = True
+  msg = messaging.new_message('liveLocationKalman')
+  msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.status = 'valid'
+  msg.liveLocationKalman.gpsOK = True
   return msg

common/params_keys.h

@@ -195,6 +195,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"MyDrivingModeAuto", PERSISTENT},
     {"TrafficLightDetectMode", PERSISTENT},
     {"SteerActuatorDelay", PERSISTENT},
+    {"SteerSmoothSec", PERSISTENT},
     {"CruiseOnDist", PERSISTENT},
     {"CruiseMaxVals1", PERSISTENT},
     {"CruiseMaxVals2", PERSISTENT},

launch_chffrplus.sh

@@ -90,7 +90,8 @@ function launch {
   fi
 
   # events language init
-  LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
+  #LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
+  LANG=$(cat /data/params/d/LanguageSetting)
   EVENTSTAT=$(git status)
 
   # events.py 한글로 변경 및 파일이 교체된 상태인지 확인

restart.sh

@@ -1,2 +1,2 @@
 git pull
-tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; tmux new -s comma -d "/data/openpilot/launch_openpilot.sh"
+tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; sleep 1;tmux new -s comma -d "/data/openpilot/launch_openpilot.sh"

selfdrive/car/card.py

@@ -162,6 +162,9 @@ class Car:
     self.is_metric = self.params.get_bool("IsMetric")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")
 
+    #self.t1 = time.monotonic()
+    #self.t2 = self.t1
+    #self.t3 = self.t2
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -182,6 +185,7 @@ class Car:
     #RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, can_list)
 
     self.sm.update(0)
+    #self.t1 = time.monotonic()
 
     can_rcv_valid = len(can_strs) > 0
 
@@ -193,9 +197,11 @@ class Car:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
     RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, rcv_time, can_list)
+    #self.t2 = time.monotonic()
 
     #self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     self.v_cruise_helper.update_v_cruise(CS, self.sm, self.is_metric)
+    #self.t3 = time.monotonic()
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
       self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
@@ -285,13 +291,15 @@ class Car:
     try:
       t.start()
       while True:
+        #start = time.monotonic()
         self.step()
+        #if self.sm.frame % 100 == 0:
+        #  print(f"elapsed time = {(self.t1 - start)*1000.:.2f}, {(self.t2 - self.t1)*1000.:.2f}, {(self.t3 - self.t1)*1000.:.2f}, {(time.monotonic() - self.t1)*1000.:.2f}")
         self.rk.monitor_time()
     finally:
       e.set()
       t.join()
-
+    
-
 def main():
   config_realtime_process(4, Priority.CTRL_HIGH)
   car = Car()

selfdrive/carrot/carrot_functions.py

@@ -80,7 +80,7 @@ class CarrotPlanner:
 
     self.stop_distance = 6.0
     self.trafficStopDistanceAdjust = 1.0 #params.get_float("TrafficStopDistanceAdjust") / 100.
-    self.comfortBrake = 2.5
+    self.comfortBrake = 2.4
     self.comfort_brake = self.comfortBrake
 
     self.soft_hold_active = 0
@@ -121,7 +121,6 @@ class CarrotPlanner:
     self.jerk_factor_apply = 1.0
 
     self.j_lead_factor = 0.0
-    self.carrot_mpc1 = 0.0
 
     self.activeCarrot = 0
     self.xDistToTurn = 0

selfdrive/carrot/carrot_man.py

@@ -238,7 +238,7 @@ class CarrotMan:
     print("************************************************CarrotMan init************************************************")
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
-    self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'navInstruction'])
+    self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman', 'navInstruction'])
     self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
 
     self.carrot_serv = CarrotServ()
@@ -756,6 +756,11 @@ class CarrotMan:
         print("Received points from navd:", len(self.navi_points))
         self.navd_active = True
 
+        # 경로수신 -> carrotman active되고 약간의 시간지연이 발생함..
+        self.carrot_serv.active_count = 80
+        self.carrot_serv.active_sdi_count = self.carrot_serv.active_sdi_count_max
+        self.carrot_serv.active_carrot = 2
+
         coords = [{"latitude": c.latitude, "longitude": c.longitude} for c in coords]
         #print("navdNaviPoints=", self.navi_points)
       else:
@@ -1265,11 +1270,14 @@ class CarrotServ:
       self.xSpdDist = 0
 
   def _update_gps(self, v_ego, sm):
-    if not sm.updated['carState'] or not sm.updated['carControl']:
+    llk = 'liveLocationKalman'
+    location = sm[llk]
+    #print(f"location = {sm.valid[llk]}, {sm.updated[llk]}, {sm.recv_frame[llk]}, {sm.recv_time[llk]}")
+    if not sm.updated['carState'] or not sm.updated['carControl']: # or not sm.updated[llk]:
       return self.nPosAngle
     CS = sm['carState']
     CC = sm['carControl']
-    self.gps_valid = False #(location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
+    self.gps_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
 
     now = time.monotonic()
     gps_updated_phone = (now - self.last_update_gps_time_phone) < 3
@@ -1278,8 +1286,8 @@ class CarrotServ:
     bearing = self.nPosAngle
     if gps_updated_phone:
       self.bearing_offset = 0.0
-    elif len(CC.orientationNED) == 3:
+    elif sm.valid[llk]:
-      bearing = math.degrees(CC.orientationNED[2])
+      bearing = math.degrees(location.calibratedOrientationNED.value[2])
       if self.gps_valid:
         self.bearing_offset = 0.0
       elif self.active_carrot > 0:
@@ -1293,7 +1301,7 @@ class CarrotServ:
 
     external_gps_update_timedout = not (gps_updated_phone or gps_updated_navi)
     #print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}")
-    if False: #self.gps_valid and external_gps_update_timedout:    # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
+    if self.gps_valid and external_gps_update_timedout:    # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
       self.vpPosPointLatNavi = location.positionGeodetic.value[0]
       self.vpPosPointLonNavi = location.positionGeodetic.value[1]
       self.last_calculate_gps_time = now #sm.recv_time[llk]

selfdrive/carrot_settings.json

@@ -213,15 +213,28 @@
       "group": "조향튜닝",
       "name": "SteerActuatorDelay",
       "title": "SteerActuatorDelay(30)",
-      "descr": "조향지연값",
+      "descr": "조향지연값, 0:LiveDelay",
       "egroup": "LAT",
       "etitle": "SteerActuatorDelay(30)",
-      "edescr": "",
+      "edescr": "0:LiveDelay",
-      "min": 1,
+      "min": 0,
       "max": 300,
       "default": 30,
       "unit": 1
     },
+    {
+      "group": "조향튜닝",
+      "name": "SteerSmoothSec",
+      "title": "조향필터링타임x0.01(13)",
+      "descr": "값이커지면 필터링을 더 많이 함.",
+      "egroup": "LAT",
+      "etitle": "SteerSmoothSecx0.01(13)",
+      "edescr": "",
+      "min": 1,
+      "max": 100,
+      "default": 13,
+      "unit": 1
+    },
     {
       "group": "크루즈",
       "name": "CruiseOnDist",

selfdrive/controls/controlsd.py

@@ -20,13 +20,11 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
-from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 
 from openpilot.common.realtime import DT_CTRL, DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 
 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
@@ -47,8 +45,8 @@ class Controls:
     self.disable_dm = False
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
+                                   'liveCalibration', 'liveLocationKalman', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'carrotMan', 'lateralPlan', 'radarState',
+                                   'liveDelay', 'carrotMan', 'lateralPlan', 'radarState',
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
@@ -57,9 +55,6 @@ class Controls:
     self.desired_curvature = 0.0
     self.yStd = 0.0
 
-    self.pose_calibrator = PoseCalibrator()
-    self.calibrated_pose: Pose | None = None
-
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
@@ -72,11 +67,6 @@ class Controls:
 
   def update(self):
     self.sm.update(15)
-    if self.sm.updated["liveCalibration"]:
-      self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
-    if self.sm.updated["livePose"]:
-      device_pose = Pose.from_live_pose(self.sm['livePose'])
-      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
 
   def state_control(self):
     CS = self.sm['carState']
@@ -143,11 +133,13 @@ class Controls:
     curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed)
     self.lanefull_mode_enabled = (lat_plan.useLaneLines and self.params.get_int("UseLaneLineSpeedApply") > 0 and
                                   curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
-
+    lat_smooth_seconds = self.params.get_float("SteerSmoothSec") * 0.01
-    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS
+    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
+    if steer_actuator_delay == 0.0:
+      steer_actuator_delay = self.sm['liveDelay'].lateralDelay 
 
     if len(model_v2.position.yStd) > 0:
-      yStd = np.interp(steer_actuator_delay + LAT_SMOOTH_SECONDS, ModelConstants.T_IDXS, model_v2.position.yStd)
+      yStd = np.interp(steer_actuator_delay + lat_smooth_seconds, ModelConstants.T_IDXS, model_v2.position.yStd)
       self.yStd = yStd * 0.1 + self.yStd * 0.9
     else:
       self.yStd = 0.0
@@ -163,8 +155,8 @@ class Controls:
           return alpha * val + (1 - alpha) * prev_val
 
         t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL
-        curvature = np.interp(steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)          
+        curvature = np.interp(steer_actuator_delay + lat_smooth_seconds + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)          
-        new_desired_curvature = smooth_value(curvature, self.desired_curvature, LAT_SMOOTH_SECONDS)
+        new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
     else:
       new_desired_curvature = model_v2.action.desiredCurvature
 
@@ -173,7 +165,7 @@ class Controls:
     actuators.curvature = float(self.desired_curvature)
     steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                        self.steer_limited_by_controls, self.desired_curvature,
-                                                       self.calibrated_pose, curvature_limited)  # TODO what if not available
+                                                       self.sm['liveLocationKalman'], curvature_limited)  # TODO what if not available
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
     actuators.yStd = float(self.yStd)
@@ -194,9 +186,12 @@ class Controls:
 
     # Orientation and angle rates can be useful for carcontroller
     # Only calibrated (car) frame is relevant for the carcontroller
-    if self.calibrated_pose is not None:
+    orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
-      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+    if len(orientation_value) > 2:
-      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
+      CC.orientationNED = orientation_value
+    angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
+    if len(angular_rate_value) > 2:
+      CC.angularVelocity = angular_rate_value
 
     CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)

selfdrive/controls/lib/latcontrol.py

@@ -17,7 +17,7 @@ class LatControl(ABC):
     self.steer_max = 1.0
 
   @abstractmethod
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
     pass
 
   def reset(self):

selfdrive/controls/lib/latcontrol_angle.py

@@ -13,7 +13,7 @@ class LatControlAngle(LatControl):
     self.sat_check_min_speed = 5.
     self.angle_steers_des = 0.0
 
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if not active:

selfdrive/controls/lib/latcontrol_pid.py

@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
     super().reset()
     self.pid.reset()
 
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)

selfdrive/controls/lib/latcontrol_torque.py

@@ -57,7 +57,7 @@ class LatControlTorque(LatControl):
     self.torque_params.latAccelOffset = latAccelOffset
     self.torque_params.friction = friction
 
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
     self.frame += 1
     if self.frame % 10 == 0:
       lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
@@ -94,9 +94,8 @@ class LatControlTorque(LatControl):
         actual_curvature = actual_curvature_vm
         curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
       else:
-        assert calibrated_pose is not None
+        actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
-        actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
+        actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
-        actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
         curvature_deadzone = 0.0
       desired_lateral_accel = desired_curvature * CS.vEgo ** 2
 

selfdrive/controls/radard.py

@@ -453,8 +453,10 @@ class RadarD:
 
     v_ego = self.v_ego
     ready = self.ready
-    ## SCC레이더는 일단 보관하고 리스트에서 삭제...
+    ## SCC레이더는 일단 보관하고 리스트에서 삭제... (SCC Track은 0,1번으로 들어옴)
     track_scc = tracks.get(0)
+    if track_scc is None:
+      track_scc = tracks.get(1)
     #if track_scc is not None:
     #  del tracks[0]            ## tracks에서 삭제하면안됨... ㅠㅠ
 

selfdrive/locationd/SConscript

@@ -1,15 +1,17 @@
-Import('env', 'rednose')
+Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
+
+loc_libs = [messaging, common, 'pthread', 'dl']
 
 # build ekf models
 rednose_gen_dir = 'models/generated'
 rednose_gen_deps = [
   "models/constants.py",
 ]
-pose_ekf = env.RednoseCompileFilter(
+live_ekf = env.RednoseCompileFilter(
-  target='pose',
+  target='live',
-  filter_gen_script='models/pose_kf.py',
+  filter_gen_script='models/live_kf.py',
   output_dir=rednose_gen_dir,
-  extra_gen_artifacts=[],
+  extra_gen_artifacts=['live_kf_constants.h'],
   gen_script_deps=rednose_gen_deps,
 )
 car_ekf = env.RednoseCompileFilter(
@@ -19,3 +21,17 @@ car_ekf = env.RednoseCompileFilter(
   extra_gen_artifacts=[],
   gen_script_deps=rednose_gen_deps,
 )
+
+# locationd build
+locationd_sources = ["locationd.cc", "models/live_kf.cc"]
+
+lenv = env.Clone()
+# ekf filter libraries need to be linked, even if no symbols are used
+if arch != "Darwin":
+  lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
+
+lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
+lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
+locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
+lenv.Depends(locationd, rednose)
+lenv.Depends(locationd, live_ekf)

selfdrive/locationd/lagd.py

@@ -11,7 +11,7 @@ from cereal.services import SERVICE_LIST
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
+from openpilot.selfdrive.locationd.helpers import fft_next_good_size, parabolic_peak_interp
 
 BLOCK_SIZE = 100
 BLOCK_NUM = 50
@@ -106,6 +106,8 @@ class Points:
     self.okay.append(okay)
     self.desired.append(desired)
     self.actual.append(actual)
+    #if okay:
+    #  print(f"lagd: {t:.3f} {desired:.4f} {actual:.4f} {self.num_okay}/{self.num_points}")
 
   def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
     return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay)
@@ -148,7 +150,7 @@ class BlockAverage:
 
 
 class LateralLagEstimator:
-  inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"}
+  inputs = {"carControl", "carState", "controlsState", "liveLocationKalman"}
 
   def __init__(self, CP: car.CarParams, dt: float,
                block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
@@ -185,7 +187,7 @@ class LateralLagEstimator:
     self.last_pose_invalid_t = 0.0
     self.last_estimate_t = 0.0
 
-    self.calibrator = PoseCalibrator()
+    self.calib_valid = False
 
     self.reset(self.initial_lag, 0)
 
@@ -237,14 +239,11 @@ class LateralLagEstimator:
     elif which == "controlsState":
       self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
       self.desired_curvature = msg.desiredCurvature
-    elif which == "liveCalibration":
+    elif which == 'liveLocationKalman':
-      self.calibrator.feed_live_calib(msg)
+      self.yaw_rate = msg.angularVelocityCalibrated.value[2]
-    elif which == "livePose":
+      self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]    
-      device_pose = Pose.from_live_pose(msg)
+      self.pose_valid = msg.angularVelocityCalibrated.valid and msg.posenetOK and msg.inputsOK  
-      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.calib_valid = msg.status == log.LiveLocationKalman.Status.valid
-      self.yaw_rate = calibrated_pose.angular_velocity.yaw
-      self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
-      self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
     self.t = t
 
   def points_enough(self):
@@ -261,7 +260,7 @@ class LateralLagEstimator:
     turning = np.abs(self.yaw_rate) >= self.min_yr
     sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK
     la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff
-    calib_valid = self.calibrator.calib_valid
+    calib_valid = self.calib_valid #self.calibrator.calib_valid
 
     if not self.lat_active:
       self.last_lat_inactive_t = self.t
@@ -343,12 +342,12 @@ def main():
   DEBUG = bool(int(os.getenv("DEBUG", "0")))
 
   pm = messaging.PubMaster(['liveDelay'])
-  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose')
+  sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'controlsState', 'carControl'], poll='liveLocationKalman')
 
   params_reader = Params()
   CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
 
-  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
+  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['liveLocationKalman'].frequency)
   if (initial_lag_params := retrieve_initial_lag(params_reader, CP)) is not None:
     lag, valid_blocks = initial_lag_params
     lag_learner.reset(lag, valid_blocks)

selfdrive/locationd/models/car_kf.py

@@ -160,18 +160,19 @@ class CarKalman(KalmanFilter):
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
 
-  def __init__(self, generated_dir):
+  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
-    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    dim_state = self.initial_x.shape[0]
-    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+    dim_state_err = self.P_initial.shape[0]
-                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+    x_init = self.initial_x
+    x_init[States.STEER_RATIO] = steer_ratio
+    x_init[States.STIFFNESS] = stiffness_factor
+    x_init[States.ANGLE_OFFSET] = angle_offset
 
-  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    if P_initial is not None:
-    self.filter.set_global("mass", mass)
+      self.P_initial = P_initial
-    self.filter.set_global("rotational_inertia", rotational_inertia)
+    # init filter
-    self.filter.set_global("center_to_front", center_to_front)
+    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
-    self.filter.set_global("center_to_rear", center_to_rear)
+                              dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
-    self.filter.set_global("stiffness_front", stiffness_front)
-    self.filter.set_global("stiffness_rear", stiffness_rear)
 
 
 if __name__ == "__main__":

selfdrive/locationd/paramsd.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 import os
+import math
 import json
 import numpy as np
-import capnp
 
 import cereal.messaging as messaging
 from cereal import car, log
@@ -10,14 +10,14 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
-from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog
 
+
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
-ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
-ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
+ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
-ROLL_LOWERED_MAX = np.radians(8)
+ROLL_LOWERED_MAX = math.radians(8)
-ROLL_STD_MAX = np.radians(1.5)
+ROLL_STD_MAX = math.radians(1.5)
 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
 OFFSET_MAX = 10.0
 OFFSET_LOWERED_MAX = 8.0
@@ -25,61 +25,32 @@ MIN_ACTIVE_SPEED = 1.0
 LOW_ACTIVE_SPEED = 10.0
 
 
-class VehicleParamsLearner:
+class ParamsLearner:
-  def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
+  def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
-    self.kf = CarKalman(GENERATED_DIR)
+    self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
 
-    self.x_initial = CarKalman.initial_x.copy()
+    self.kf.filter.set_global("mass", CP.mass)
-    self.x_initial[States.STEER_RATIO] = steer_ratio
+    self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
-    self.x_initial[States.STIFFNESS] = stiffness_factor
+    self.kf.filter.set_global("center_to_front", CP.centerToFront)
-    self.x_initial[States.ANGLE_OFFSET] = angle_offset
+    self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
-    self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
+    self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
+    self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
 
-    self.kf.set_globals(
+    self.active = False
-      mass=CP.mass,
-      rotational_inertia=CP.rotationalInertia,
-      center_to_front=CP.centerToFront,
-      center_to_rear=CP.wheelbase - CP.centerToFront,
-      stiffness_front=CP.tireStiffnessFront,
-      stiffness_rear=CP.tireStiffnessRear
-    )
 
-    self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+    self.speed = 0.0
+    self.yaw_rate = 0.0
+    self.yaw_rate_std = 0.0
+    self.roll = 0.0
+    self.steering_angle = 0.0
 
-    self.calibrator = PoseCalibrator()
+  def handle_log(self, t, which, msg):
+    if which == 'liveLocationKalman':
+      self.yaw_rate = msg.angularVelocityCalibrated.value[2]
+      self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
 
-    self.observed_speed = 0.0
+      localizer_roll = msg.orientationNED.value[0]
-    self.observed_yaw_rate = 0.0
+      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
-    self.observed_roll = 0.0
-
-    self.avg_offset_valid = True
-    self.total_offset_valid = True
-    self.roll_valid = True
-
-    self.reset(None)
-
-  def reset(self, t: float | None):
-    self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
-
-    self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
-    self.avg_angle_offset = self.angle_offset
-
-  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
-    if which == 'livePose':
-      device_pose = Pose.from_live_pose(msg)
-      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
-
-      yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
-      yaw_rate_valid = msg.angularVelocityDevice.valid
-      yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
-      yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
-      if not yaw_rate_valid:
-        # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
-        yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
-      self.observed_yaw_rate = yaw_rate
-
-      localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
-      localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
       roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
       if roll_valid:
         roll = localizer_roll
@@ -89,18 +60,18 @@ class VehicleParamsLearner:
         # This is done to bound the road roll estimate when localizer values are invalid
         roll = 0.0
         roll_std = np.radians(10.0)
-      self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
+      self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
 
       if self.active:
         if msg.posenetOK:
           self.kf.predict_and_observe(t,
                                       ObservationKind.ROAD_FRAME_YAW_RATE,
-                                      np.array([[-self.observed_yaw_rate]]),
+                                      np.array([[-self.yaw_rate]]),
-                                      np.array([np.atleast_2d(yaw_rate_std**2)]))
+                                      np.array([np.atleast_2d(self.yaw_rate_std**2)]))
 
           self.kf.predict_and_observe(t,
                                       ObservationKind.ROAD_ROLL,
-                                      np.array([[self.observed_roll]]),
+                                      np.array([[self.roll]]),
                                       np.array([np.atleast_2d(roll_std**2)]))
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
@@ -112,83 +83,21 @@ class VehicleParamsLearner:
         self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
         self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
 
-    elif which == 'liveCalibration':
-      self.calibrator.feed_live_calib(msg)
-
     elif which == 'carState':
-      steering_angle = msg.steeringAngleDeg
+      self.steering_angle = msg.steeringAngleDeg
+      self.speed = msg.vEgo
 
-      in_linear_region = abs(steering_angle) < 45
+      in_linear_region = abs(self.steering_angle) < 45
-      self.observed_speed = msg.vEgo
+      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
-      self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
 
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
-        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
 
     if not self.active:
       # Reset time when stopped so uncertainty doesn't grow
-      self.kf.filter.set_filter_time(t)  # type: ignore
+      self.kf.filter.set_filter_time(t)
-      self.kf.filter.reset_rewind()      # type: ignore
+      self.kf.filter.reset_rewind()
-
-  def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
-    x = self.kf.x
-    P = np.sqrt(self.kf.P.diagonal())
-    if not np.all(np.isfinite(x)):
-      cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
-      self.reset(self.kf.t)
-      x = self.kf.x
-
-    self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
-                                self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
-    self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
-                        self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
-    self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
-    roll_std = float(P[States.ROAD_ROLL].item())
-    if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
-      # Account for the opposite signs of the yaw rates
-      # At low speeds, bumping into a curb can cause the yaw rate to be very high
-      sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
-    else:
-      sensors_valid = True
-    self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-    self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-    self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
-
-    msg = messaging.new_message('liveParameters')
-
-    msg.valid = valid
-
-    liveParameters = msg.liveParameters
-    liveParameters.posenetValid = True
-    liveParameters.sensorValid = sensors_valid
-    liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
-    liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-    liveParameters.roll = float(self.roll)
-    liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
-    liveParameters.angleOffsetDeg = float(self.angle_offset)
-    liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
-    liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
-    liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
-    liveParameters.angleOffsetValid = bool(self.total_offset_valid)
-    liveParameters.valid = all((
-      liveParameters.angleOffsetAverageValid,
-      liveParameters.angleOffsetValid ,
-      self.roll_valid,
-      roll_std < ROLL_STD_MAX,
-      liveParameters.stiffnessFactorValid,
-      liveParameters.steerRatioValid,
-    ))
-    liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
-    liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
-    liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
-    liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
-    if debug:
-      liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
-      liveParameters.debugFilterState.value = x.tolist()
-      liveParameters.debugFilterState.std = P.tolist()
-
-    return msg
 
 
 def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
@@ -199,67 +108,6 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
   return current_valid
 
 
-# TODO: Remove this function after few releases (added in 0.9.9)
-def migrate_cached_vehicle_params_if_needed(params: Params):
-  last_parameters_data_old = params.get("LiveParameters")
-  last_parameters_data = params.get("LiveParametersV2")
-  if last_parameters_data_old is None or last_parameters_data is not None:
-    return
-
-  try:
-    last_parameters_dict = json.loads(last_parameters_data_old)
-    last_parameters_msg = messaging.new_message('liveParameters')
-    last_parameters_msg.liveParameters.valid = True
-    last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio']
-    last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor']
-    last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg']
-    params.put("LiveParametersV2", last_parameters_msg.to_bytes())
-  except Exception as e:
-    cloudlog.error(f"Failed to perform parameter migration: {e}")
-    params.remove("LiveParameters")
-
-
-def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
-  last_parameters_data = params.get("LiveParametersV2")
-  last_carparams_data = params.get("CarParamsPrevRoute")
-
-  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
-
-  retrieve_success = False
-  if last_parameters_data is not None and last_carparams_data is not None:
-    try:
-      with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
-        lp = last_lp_msg.liveParameters
-        # Check if car model matches
-        if last_CP.carFingerprint != CP.carFingerprint:
-          raise Exception("Car model mismatch")
-
-        # Check if starting values are sane
-        min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
-        steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
-        if not steer_ratio_sane:
-          raise Exception(f"Invalid starting values found {lp}")
-
-        initial_filter_std = np.array(lp.debugFilterState.std)
-        if debug and len(initial_filter_std) != 0:
-          p_initial = np.diag(initial_filter_std)
-
-        steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
-        retrieve_success = True
-    except Exception as e:
-      cloudlog.error(f"Failed to retrieve initial values: {e}")
-
-  if not replay:
-    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
-    # Without a way to detect this we have to reset the stiffness every drive
-    stiffness_factor = 1.0
-
-  if not retrieve_success:
-    cloudlog.info("Parameter learner resetting to default values")
-
-  return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
-
-
 def main():
   config_realtime_process([0, 1, 2, 3], 5)
 
@@ -267,15 +115,64 @@ def main():
   REPLAY = bool(int(os.getenv("REPLAY", "0")))
 
   pm = messaging.PubMaster(['liveParameters'])
-  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
+  sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman')
 
-  params = Params()
+  params_reader = Params()
-  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+  # wait for stats about the car to come in from controls
+  cloudlog.info("paramsd is waiting for CarParams")
+  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
+  cloudlog.info("paramsd got CarParams")
 
-  migrate_cached_vehicle_params_if_needed(params)
+  min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
 
-  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
+  params = params_reader.get("LiveParameters")
-  learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
+
+  # Check if car model matches
+  if params is not None:
+    params = json.loads(params)
+    if params.get('carFingerprint', None) != CP.carFingerprint:
+      cloudlog.info("Parameter learner found parameters for wrong car.")
+      params = None
+
+  # Check if starting values are sane
+  if params is not None:
+    try:
+      steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
+      if not steer_ratio_sane:
+        cloudlog.info(f"Invalid starting values found {params}")
+        params = None
+    except Exception as e:
+      cloudlog.info(f"Error reading params {params}: {str(e)}")
+      params = None
+
+  # TODO: cache the params with the capnp struct
+  if params is None:
+    params = {
+      'carFingerprint': CP.carFingerprint,
+      'steerRatio': CP.steerRatio,
+      'stiffnessFactor': 1.0,
+      'angleOffsetAverageDeg': 0.0,
+    }
+    cloudlog.info("Parameter learner resetting to default values")
+
+  if not REPLAY:
+    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
+    # Without a way to detect this we have to reset the stiffness every drive
+    params['stiffnessFactor'] = 1.0
+
+  pInitial = None
+  if DEBUG:
+    pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None
+
+  learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial)
+  angle_offset_average = params['angleOffsetAverageDeg']
+  angle_offset = angle_offset_average
+  roll = 0.0
+  avg_offset_valid = True
+  total_offset_valid = True
+  roll_valid = True
+  params_memory = Params("/dev/shm/params")
+  params_memory.remove("LastGPSPosition")
 
   while True:
     sm.update()
@@ -285,14 +182,80 @@ def main():
           t = sm.logMonoTime[which] * 1e-9
           learner.handle_log(t, which, sm[which])
 
-    if sm.updated['livePose']:
+    if sm.updated['liveLocationKalman']:
-      msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
+      location = sm['liveLocationKalman']
+      if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
+        bearing = math.degrees(location.calibratedOrientationNED.value[2])
+        lat = location.positionGeodetic.value[0]
+        lon = location.positionGeodetic.value[1]
+        params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
+        
+      x = learner.kf.x
+      P = np.sqrt(learner.kf.P.diagonal())
+      if not all(map(math.isfinite, x)):
+        cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
+        learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
+        x = learner.kf.x
+
+      angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
+                                  angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
+      angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+                          angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
+      roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
+      roll_std = float(P[States.ROAD_ROLL].item())
+      if learner.active and learner.speed > LOW_ACTIVE_SPEED:
+        # Account for the opposite signs of the yaw rates
+        # At low speeds, bumping into a curb can cause the yaw rate to be very high
+        sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+      else:
+        sensors_valid = True
+      avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
+      total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+      roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
+
+      msg = messaging.new_message('liveParameters')
+
+      liveParameters = msg.liveParameters
+      liveParameters.posenetValid = True
+      liveParameters.sensorValid = sensors_valid
+      liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+      liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr
+      liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
+      liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
+      liveParameters.roll = float(roll)
+      liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
+      liveParameters.angleOffsetAverageValid = bool(avg_offset_valid)
+      liveParameters.angleOffsetDeg = float(angle_offset)
+      liveParameters.angleOffsetValid = bool(total_offset_valid)
+      liveParameters.valid = all((
+        liveParameters.angleOffsetAverageValid,
+        liveParameters.angleOffsetValid ,
+        roll_valid,
+        roll_std < ROLL_STD_MAX,
+        liveParameters.stiffnessFactorValid,
+        liveParameters.steerRatioValid,
+      ))
+      liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+      liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
+      if DEBUG:
+        liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
+        liveParameters.debugFilterState.value = x.tolist()
+        liveParameters.debugFilterState.std = P.tolist()
+
+      msg.valid = sm.all_checks()
 
-      msg_dat = msg.to_bytes()
       if sm.frame % 1200 == 0:  # once a minute
-        params.put_nonblocking("LiveParametersV2", msg_dat)
+        params = {
+          'carFingerprint': CP.carFingerprint,
+          'steerRatio': liveParameters.steerRatio,
+          'stiffnessFactor': liveParameters.stiffnessFactor,
+          'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
+        }
+        params_reader.put_nonblocking("LiveParameters", json.dumps(params))
 
-      pm.send('liveParameters', msg_dat)
+      pm.send('liveParameters', msg)
 
 
 if __name__ == "__main__":

selfdrive/locationd/torqued.py

@@ -9,7 +9,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -77,8 +77,6 @@ class TorqueEstimator(ParameterEstimator):
       self.offline_friction = CP.lateralTuning.torque.friction
       self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
 
-    self.calibrator = PoseCalibrator()
-
     self.reset()
 
     initial_params = {
@@ -173,18 +171,10 @@ class TorqueEstimator(ParameterEstimator):
       # TODO: check if high aEgo affects resulting lateral accel
       self.raw_points["vego"].append(msg.vEgo)
       self.raw_points["steer_override"].append(msg.steeringPressed)
-    elif which == "liveCalibration":
+    elif which == "liveLocationKalman":
-      self.calibrator.feed_live_calib(msg)
-
-    # calculate lateral accel from past steering torque
-    elif which == "livePose":
       if len(self.raw_points['steer_torque']) == self.hist_len:
-        device_pose = Pose.from_live_pose(msg)
+        yaw_rate = msg.angularVelocityCalibrated.value[2]
-        calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+        roll = msg.orientationNED.value[0]
-        angular_velocity_calibrated = calibrated_pose.angular_velocity
-
-        yaw_rate = angular_velocity_calibrated.yaw
-        roll = device_pose.orientation.roll
         # check lat active up to now (without lag compensation)
         lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
                                self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
@@ -241,7 +231,7 @@ def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
 
   params = Params()
   estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
@@ -254,7 +244,7 @@ def main(demo=False):
           t = sm.logMonoTime[which] * 1e-9
           estimator.handle_log(t, which, sm[which])
 
-    # 4Hz driven by livePose
+    # 4Hz driven by liveLocationKalman
     if sm.frame % 5 == 0:
       pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
 

selfdrive/modeld/modeld.py

@@ -48,7 +48,7 @@ LONG_SMOOTH_SECONDS = 0.3
 
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                          lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action:
+                          lat_action_t: float, long_action_t: float, lat_smooth_seconds: float) -> log.ModelDataV2.Action:
     plan = model_output['plan'][0]
     desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
                                                      plan[:,Plan.ACCELERATION][:,0],
@@ -57,7 +57,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
 
     desired_curvature = model_output['desired_curvature'][0, 0]
-    desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
+    desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth_seconds)
 
     return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
                                   desiredAcceleration=float(desired_accel),
@@ -218,7 +218,7 @@ def main(demo=False):
 
   # messaging
   pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "carrotMan", "radarState"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "carrotMan", "radarState"])
 
   publish_state = PublishState()
   params = Params()
@@ -251,8 +251,19 @@ def main(demo=False):
 
   DH = DesireHelper()
 
+  frame = 0
+  custom_lat_delay = 0.0
+  lat_smooth_seconds = LAT_SMOOTH_SECONDS
   while True:
-    lat_delay = params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS
+    frame += 1
+    if frame % 100 == 0:
+      custom_lat_delay = params.get_float("SteerActuatorDelay") * 0.01
+      lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01
+      
+    if custom_lat_delay > 0.0:
+      lat_delay = custom_lat_delay + lat_smooth_seconds
+    else:
+      lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds
 
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
     while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
@@ -335,7 +346,7 @@ def main(demo=False):
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
 
-      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL)
+      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, lat_smooth_seconds)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

selfdrive/selfdrived/selfdrived.py

@@ -82,7 +82,7 @@ class SelfdriveD:
       # no vipc in replay will make them ignored anyways
       ignore += ['roadCameraState', 'wideRoadCameraState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', # 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'carrotMan',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \
@@ -340,11 +340,13 @@ class SelfdriveD:
       self.logged_comm_issue = None
 
     if not self.CP.notCar:
-      if not self.sm['livePose'].posenetOK:
+      if not self.sm['liveLocationKalman'].posenetOK:
         self.events.add(EventName.posenetInvalid)
-      if not self.sm['livePose'].inputsOK:
+      if not self.sm['liveLocationKalman'].deviceStable:
+        self.events.add(EventName.deviceFalling)
+      if not self.sm['liveLocationKalman'].inputsOK:
         self.events.add(EventName.locationdTemporaryError)
-      if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
+      if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
         self.events.add(EventName.paramsdTemporaryError)
 
     # conservative HW alert. if the data or frequency are off, locationd will throw an error
@@ -385,7 +387,7 @@ class SelfdriveD:
     if not SIMULATION or REPLAY:
       # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
       gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
-      if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
+      if not gps_ok and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500):
         if self.distance_traveled < 1600:
           self.events.add(EventName.noGps)
       #if gps_ok:

selfdrive/ui/carrot.cc

@@ -2209,24 +2209,11 @@ public:
         if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
         strcpy(driving_mode_str_last, driving_mode_str);
 
-        if(sm.updated(s->gps_location_socket)) {
-          if (!strcmp(s->gps_location_socket, "gpsLocationExternal")) {
-            auto gpsLocation = sm[s->gps_location_socket].getGpsLocationExternal();
-            ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
-          }
-          else {
-            auto gpsLocation = sm[s->gps_location_socket].getGpsLocation();
-            ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
-          
-          }
-        }
-        /*
         auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
-        bool is_gps_valid = locationd.getGpsOK();
+        bool is_gps_valid = sm.valid("liveLocationKalman") && locationd.getGpsOK();
         if (is_gps_valid) {
           ui_draw_text(s, dx, dy - 45, "GPS", 30, COLOR_GREEN, BOLD);
         }
-        */
 
         char gap_str[32];
         int gap = params.getInt("LongitudinalPersonality") + 1;
@@ -2747,9 +2734,11 @@ public:
         str = QString::fromStdString(car_state.getLogCarrot());
         sprintf(top, "%s", str.toStdString().c_str());
         // top_right
+        const auto live_delay = sm["liveDelay"].getLiveDelay();
         const auto live_torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
         const auto live_params = sm["liveParameters"].getLiveParameters();
-        str.sprintf("LT[%.0f,%s](%.2f/%.2f), SR(%.1f,%.1f)",
+        str.sprintf("LD[%d,%.2f],LT[%.0f,%s](%.2f/%.2f), SR(%.1f,%.1f)",
+            live_delay.getValidBlocks(), live_delay.getLateralDelay(),
             live_torque_params.getTotalBucketPoints(), live_torque_params.getLiveValid() ? "ON" : "OFF",
             live_torque_params.getLatAccelFactorFiltered(), live_torque_params.getFrictionCoefficientFiltered(),
             live_params.getSteerRatio(), params.getFloat("CustomSR")/10.0);

selfdrive/ui/qt/maps/map.cc

@@ -295,7 +295,6 @@ void MapWindow::updateState(const UIState &s) {
     updateDestinationMarker();
   }
   if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) {
-    /*
     auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
     if (locationd_location.getGpsOK()) {
       //auto carrot_man = sm["carrotMan"].getCarrotMan();
@@ -307,7 +306,6 @@ void MapWindow::updateState(const UIState &s) {
       modelV2Path["data"] = QVariant::fromValue<QMapLibre::Feature>(model_path_feature);
       m_map->updateSource("modelPathSource", modelV2Path);
     }
-    */
     model_rcv_frame = sm.rcv_frame("modelV2");
   }
 }

selfdrive/ui/qt/offroad/settings.cc

@@ -692,7 +692,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
   latLongToggles->addItem(new CValueControl("CruiseMaxVals6", "ACCEL:140km/h(60)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5));
   //latLongToggles->addItem(new CValueControl("CruiseMinVals", "DECEL:(120)", "감속도를 설정합니다.(x0.01m/s^2)", "../assets/offroad/icon_road.png", 50, 250, 5));
   latLongToggles->addItem(new CValueControl("MaxAngleFrames", "MaxAngleFrames(89)", "89:기본, 스티어계기판에러시 85~87", "../assets/offroad/icon_logic.png", 80, 100, 1));
-  latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "표준", "../assets/offroad/icon_logic.png", 1, 100, 1));
+  latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "x0.01, 0:LiveDelay", "../assets/offroad/icon_logic.png", 0, 100, 1));
+  latLongToggles->addItem(new CValueControl("SteerSmoothSec", "LAT:SteerSmoothSec(13)", "x0.01", "../assets/offroad/icon_logic.png", 1, 100, 1));
   latLongToggles->addItem(new CValueControl("LateralTorqueCustom", "LAT: TorqueCustom(0)", "", "../assets/offroad/icon_logic.png", 0, 2, 1));
   latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", "LAT: TorqueAccelFactor(2500)", "", "../assets/offroad/icon_logic.png", 1000, 6000, 10));
   latLongToggles->addItem(new CValueControl("LateralTorqueFriction", "LAT: TorqueFriction(100)", "", "../assets/offroad/icon_logic.png", 0, 1000, 10));

selfdrive/ui/ui.cc

@@ -98,20 +98,13 @@ void UIState::updateStatus() {
 }
 
 UIState::UIState(QObject *parent) : QObject(parent) {
-  Params params;
-  if (params.getBool("UbloxAvailable")) {
-    strcpy(gps_location_socket, "gpsLocationExternal");
-  }
-  else {
-    strcpy(gps_location_socket, "gpsLocation");
-  }
   sm = std::make_unique<SubMaster>(std::vector<const char*>{
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "longitudinalPlan",
     "carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
-    "navRoute", "navInstruction", "navInstructionCarrot", gps_location_socket,
+    "navRoute", "navInstruction", "navInstructionCarrot", "liveLocationKalman", "liveDelay",
   });
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));

selfdrive/ui/ui.h

@@ -98,8 +98,6 @@ public:
   float show_brightness_ratio = 1.0;
   int show_brightness_timer = 20;
 
-  char gps_location_socket[32] = "gpsLocationExternal";
-
 signals:
   void uiUpdate(const UIState &s);
   void offroadTransition(bool offroad);

system/manager/manager.py

@@ -145,7 +145,8 @@ def get_default_params():
     ("CustomSteerDeltaUp", "0"),
     ("CustomSteerDeltaDown", "0"),
     ("SpeedFromPCM", "2"),
-    ("SteerActuatorDelay", "30"),
+    ("SteerActuatorDelay", "0"),
+    ("SteerSmoothSec", "13"),
     ("MaxTimeOffroadMin", "60"),
     ("DisableDM", "0"),
     ("RecordRoadCam", "0"),

system/manager/process_config.py

@@ -94,7 +94,8 @@ procs = [
   NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
   PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
-  PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
+  NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad),
+  #PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
   NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
   PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
   PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
@@ -108,7 +109,7 @@ procs = [
   PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
   PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
-  #PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
+  PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
   NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
   PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
   PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),

system/qcomgpsd/qcomgpsd.py

@@ -333,7 +333,9 @@ def main() -> NoReturn:
       pm.send('qcomGnss', msg)
     elif log_type == LOG_GNSS_POSITION_REPORT:
       report = unpack_position(log_payload)
+      #print(report)
       if report["u_PosSource"] != 2:
+        print("u_PosSource =", report["u_PosSource"])
         continue
       vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
       vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]