From f97149e1b55b42b946773ffec5456e74035a191f Mon Sep 17 00:00:00 2001 From: carrot <43668841+ajouatom@users.noreply.github.com> Date: Sun, 4 May 2025 15:56:43 +0900 Subject: [PATCH] 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 | 3 + cereal/services.py | 5 +- common/mock/__init__.py | 4 +- common/mock/generators.py | 26 +- common/params_keys.h | 1 + launch_chffrplus.sh | 3 +- restart.sh | 2 +- selfdrive/car/card.py | 12 +- selfdrive/carrot/carrot_functions.py | 3 +- selfdrive/carrot/carrot_man.py | 20 +- selfdrive/carrot_settings.json | 19 +- selfdrive/controls/controlsd.py | 37 +- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 7 +- selfdrive/controls/radard.py | 4 +- selfdrive/locationd/SConscript | 26 +- selfdrive/locationd/lagd.py | 27 +- selfdrive/locationd/models/car_kf.py | 23 +- selfdrive/locationd/paramsd.py | 367 +++++++++----------- selfdrive/locationd/torqued.py | 22 +- selfdrive/modeld/modeld.py | 21 +- selfdrive/selfdrived/selfdrived.py | 12 +- selfdrive/ui/carrot.cc | 19 +- selfdrive/ui/qt/maps/map.cc | 2 - selfdrive/ui/qt/offroad/settings.cc | 3 +- selfdrive/ui/ui.cc | 9 +- selfdrive/ui/ui.h | 2 - system/manager/manager.py | 3 +- system/manager/process_config.py | 5 +- system/qcomgpsd/qcomgpsd.py | 2 + 32 files changed, 348 insertions(+), 347 deletions(-) diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc index 1a7a489..a3a4049 100644 --- a/cereal/messaging/socketmaster.cc +++ b/cereal/messaging/socketmaster.cc @@ -186,6 +186,9 @@ SubMaster::~SubMaster() { PubMaster::PubMaster(const std::vector &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); diff --git a/cereal/services.py b/cereal/services.py index a44576d..b899cd0 100644 --- a/cereal/services.py +++ b/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), diff --git a/common/mock/__init__.py b/common/mock/__init__.py index 4b01dfe..8c86bbd 100644 --- a/common/mock/__init__.py +++ b/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 } diff --git a/common/mock/generators.py b/common/mock/generators.py index 5cd9c88..40951fa 100644 --- a/common/mock/generators.py +++ b/common/mock/generators.py @@ -1,14 +1,20 @@ from cereal import messaging -def generate_livePose(): - msg = messaging.new_message('livePose') - 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 - msg.livePose.velocityDevice = meas - msg.livePose.angularVelocityDevice = meas - msg.livePose.accelerationDevice = meas - msg.livePose.inputsOK = True - msg.livePose.posenetOK = True - msg.livePose.sensorsOK = True +LOCATION1 = (32.7174, -117.16277) +LOCATION2 = (32.7558, -117.2037) + +LLK_DECIMATION = 10 +RENDER_FRAMES = 15 +DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION + + +def generate_liveLocationKalman(location=LOCATION1): + 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 diff --git a/common/params_keys.h b/common/params_keys.h index 8afc502..22b05ea 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -195,6 +195,7 @@ inline static std::unordered_map keys = { {"MyDrivingModeAuto", PERSISTENT}, {"TrafficLightDetectMode", PERSISTENT}, {"SteerActuatorDelay", PERSISTENT}, + {"SteerSmoothSec", PERSISTENT}, {"CruiseOnDist", PERSISTENT}, {"CruiseMaxVals1", PERSISTENT}, {"CruiseMaxVals2", PERSISTENT}, diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index ef09bce..11c3649 100755 --- a/launch_chffrplus.sh +++ b/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 한글로 변경 및 파일이 교체된 상태인지 확인 diff --git a/restart.sh b/restart.sh index c3ca029..0926320 100755 --- a/restart.sh +++ b/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" diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index a193d93..c6a0980 100644 --- a/selfdrive/car/card.py +++ b/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() diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index 26538c2..5df473e 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/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 diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index deb13b0..0591a71 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/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: - bearing = math.degrees(CC.orientationNED[2]) + elif sm.valid[llk]: + 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] diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 5513467..dd5325a 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -213,15 +213,28 @@ "group": "조향튜닝", "name": "SteerActuatorDelay", "title": "SteerActuatorDelay(30)", - "descr": "조향지연값", + "descr": "조향지연값, 0:LiveDelay", "egroup": "LAT", "etitle": "SteerActuatorDelay(30)", - "edescr": "", - "min": 1, + "edescr": "0:LiveDelay", + "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", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3a57eae..350fea7 100644 --- a/selfdrive/controls/controlsd.py +++ b/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', - 'carrotMan', 'lateralPlan', 'radarState', + 'liveCalibration', 'liveLocationKalman', 'longitudinalPlan', 'carState', 'carOutput', + '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")) - - steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS + lat_smooth_seconds = self.params.get_float("SteerSmoothSec") * 0.01 + 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) - new_desired_curvature = smooth_value(curvature, self.desired_curvature, LAT_SMOOTH_SECONDS) + 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) 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: - CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() - CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() + orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) + if len(orientation_value) > 2: + 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) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index dcf0003..513f2bf 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/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): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index fd80a55..1461044 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1f11995..432bf63 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f0cdfb2..ea8a304 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo - actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) + actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) curvature_deadzone = 0.0 desired_lateral_accel = desired_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f1633ec..3850b2c 100644 --- a/selfdrive/controls/radard.py +++ b/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에서 삭제하면안됨... ㅠㅠ diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index e8eeff7..27cd4d5 100644 --- a/selfdrive/locationd/SConscript +++ b/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( - target='pose', - filter_gen_script='models/pose_kf.py', +live_ekf = env.RednoseCompileFilter( + target='live', + 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) diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index f4f46b7..e367429 100644 --- a/selfdrive/locationd/lagd.py +++ b/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": - self.calibrator.feed_live_calib(msg) - elif which == "livePose": - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - 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 + elif which == 'liveLocationKalman': + self.yaw_rate = msg.angularVelocityCalibrated.value[2] + self.yaw_rate_std = msg.angularVelocityCalibrated.std[2] + self.pose_valid = msg.angularVelocityCalibrated.valid and msg.posenetOK and msg.inputsOK + self.calib_valid = msg.status == log.LiveLocationKalman.Status.valid 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) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 349897f..5f40f19 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/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): - dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0] - self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, - dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) + def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None): + dim_state = self.initial_x.shape[0] + dim_state_err = self.P_initial.shape[0] + 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): - self.filter.set_global("mass", mass) - self.filter.set_global("rotational_inertia", rotational_inertia) - self.filter.set_global("center_to_front", center_to_front) - self.filter.set_global("center_to_rear", center_to_rear) - self.filter.set_global("stiffness_front", stiffness_front) - self.filter.set_global("stiffness_rear", stiffness_rear) + if P_initial is not None: + self.P_initial = P_initial + # init filter + self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, + dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog) if __name__ == "__main__": diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index a7712ba..e2f1e11 100644 --- a/selfdrive/locationd/paramsd.py +++ b/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_MIN, ROLL_MAX = np.radians(-10), np.radians(10) -ROLL_LOWERED_MAX = np.radians(8) -ROLL_STD_MAX = np.radians(1.5) +ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) +ROLL_LOWERED_MAX = math.radians(8) +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: - def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): - self.kf = CarKalman(GENERATED_DIR) +class ParamsLearner: + def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None): + self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial) - self.x_initial = CarKalman.initial_x.copy() - self.x_initial[States.STEER_RATIO] = steer_ratio - self.x_initial[States.STIFFNESS] = stiffness_factor - self.x_initial[States.ANGLE_OFFSET] = angle_offset - self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial + self.kf.filter.set_global("mass", CP.mass) + self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) + self.kf.filter.set_global("center_to_front", CP.centerToFront) + self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront) + self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront) + self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) - self.kf.set_globals( - 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.active = False - 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 - self.observed_yaw_rate = 0.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 + localizer_roll = msg.orientationNED.value[0] + localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] 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([np.atleast_2d(yaw_rate_std**2)])) + np.array([[-self.yaw_rate]]), + 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 - self.observed_speed = msg.vEgo - self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region + in_linear_region = abs(self.steering_angle) < 45 + self.active = self.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.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]])) + 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.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.reset_rewind() # type: ignore - - 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 + self.kf.filter.set_filter_time(t) + self.kf.filter.reset_rewind() 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() - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + params_reader = Params() + # 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) - learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) + params = params_reader.get("LiveParameters") + + # 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']: - msg = learner.get_msg(sm.all_checks(), debug=DEBUG) + if sm.updated['liveLocationKalman']: + 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__": diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index b430dfc..0d45a25 100644 --- a/selfdrive/locationd/torqued.py +++ b/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": - self.calibrator.feed_live_calib(msg) - - # calculate lateral accel from past steering torque - elif which == "livePose": + elif which == "liveLocationKalman": if len(self.raw_points['steer_torque']) == self.hist_len: - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - angular_velocity_calibrated = calibrated_pose.angular_velocity - - yaw_rate = angular_velocity_calibrated.yaw - roll = device_pose.orientation.roll + yaw_rate = msg.angularVelocityCalibrated.value[2] + roll = msg.orientationNED.value[0] # 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())) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index fd469e5..ae31219 100755 --- a/selfdrive/modeld/modeld.py +++ b/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, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index c4cd8a5..afc896c 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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: diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index f2dcef6..8910cf9 100644 --- a/selfdrive/ui/carrot.cc +++ b/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); diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index b23a4af..8abfce5 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/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(model_path_feature); m_map->updateSource("modelPathSource", modelV2Path); } - */ model_rcv_frame = sm.rcv_frame("modelV2"); } } diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index f0777f6..d687bb3 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/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)); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index d9ff4bd..9e6e0e6 100644 --- a/selfdrive/ui/ui.cc +++ b/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(std::vector{ "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")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index b944da7..884e6bd 100644 --- a/selfdrive/ui/ui.h +++ b/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); diff --git a/system/manager/manager.py b/system/manager/manager.py index bdc3e0a..2933cc1 100755 --- a/system/manager/manager.py +++ b/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"), diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 25dce84..9787c2d 100644 --- a/system/manager/process_config.py +++ b/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), diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index 3a9e358..9a6fae3 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/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]"]]