diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index fa185aa483..7b93e4aa73 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -763,7 +763,7 @@ struct RadarState @0x9a185389d6fdd05f { } } -struct LiveCalibrationData { +struct ExtrinsicsCalibration @0x96df70754d8390bc { calStatus @11 :Status; calCycle @2 :Int32; calPerc @3 :Int8; @@ -1384,7 +1384,7 @@ struct LiveLocationKalman { } -struct LivePose { +struct DeviceMotion @0xc24ca2b57206b44d { # More info on reference frames: # https://github.com/commaai/openpilot/tree/master/openpilot/common/transformations orientationNED @0 :XYZMeasurement; @@ -2266,7 +2266,7 @@ struct Boot { } } -struct LiveParametersData { +struct VehicleParameters @0xd9058dcb967c2753 { valid @0 :Bool; gyroBias @1 :Float32; angleOffsetDeg @2 :Float32; @@ -2300,8 +2300,8 @@ struct LiveParametersData { } } -struct LiveTorqueParametersData { - liveValid @0 :Bool; +struct LateralTorqueParameters @0xe61690eb0b091692 { + valid @0 :Bool; latAccelFactorRaw @1 :Float32; latAccelOffsetRaw @2 :Float32; frictionCoefficientRaw @3 :Float32; @@ -2317,7 +2317,7 @@ struct LiveTorqueParametersData { calPerc @13 :Int8; } -struct LiveDelayData { +struct LateralDelay @0x98dfdb22c44df8d4 { lateralDelay @0 :Float32; validBlocks @1 :Int32; status @2 :Status; @@ -2535,9 +2535,9 @@ struct Event { pandaStates @81 :List(PandaState); peripheralState @80 :PeripheralState; radarState @13 :RadarState; - liveTracks @131 :Car.RadarData; + radarTracks @131 :Car.RadarData; sendcan @17 :List(CanData); - liveCalibration @19 :LiveCalibrationData; + extrinsicsCalibration @19 :ExtrinsicsCalibration; carState @22 :Car.CarState; carControl @23 :Car.CarControl; carOutput @127 :Car.CarOutput; @@ -2548,15 +2548,15 @@ struct Event { qcomGnss @31 :QcomGnss; gpsLocationExternal @48 :GpsLocationData; gpsLocation @21 :GpsLocationData; - liveParameters @61 :LiveParametersData; - liveTorqueParameters @94 :LiveTorqueParametersData; - liveDelay @146 : LiveDelayData; + vehicleParameters @61 :VehicleParameters; + lateralTorqueParameters @94 :LateralTorqueParameters; + lateralDelay @146 : LateralDelay; cameraOdometry @63 :CameraOdometry; thumbnail @66: Thumbnail; onroadEvents @134: List(OnroadEvent); carParams @69: Car.CarParams; driverMonitoringState @151 :DriverMonitoringState; - livePose @129 :LivePose; + deviceMotion @129 :DeviceMotion; modelV2 @75 :ModelDataV2; drivingModelData @128 :DrivingModelData; driverStateV2 @92 :DriverStateV2; diff --git a/openpilot/cereal/messaging/tests/test_pub_sub_master.py b/openpilot/cereal/messaging/tests/test_pub_sub_master.py index c9304a3e65..24ee68d4fd 100644 --- a/openpilot/cereal/messaging/tests/test_pub_sub_master.py +++ b/openpilot/cereal/messaging/tests/test_pub_sub_master.py @@ -70,7 +70,7 @@ class TestSubMaster(OpenpilotTestCase): def test_avg_frequency_checks(self): for poll in (True, False): - sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"], + sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "extrinsicsCalibration"], poll=("modelV2" if poll else None), frequency=(20. if not poll else None)) @@ -78,7 +78,7 @@ class TestSubMaster(OpenpilotTestCase): "carState": (20, 20), "modelV2": (20, 20 if poll else 10), "cameraOdometry": (20, 10), - "liveCalibration": (4, 4), + "extrinsicsCalibration": (4, 4), "carParams": (None, None), "userBookmark": (None, None), } diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index 61fb8fcbd2..ebe8be1d60 100755 --- a/openpilot/cereal/services.py +++ b/openpilot/cereal/services.py @@ -34,13 +34,13 @@ _services: dict[str, tuple] = { "peripheralState": (True, 2., 1), "radarState": (True, 20., 5), "narrowRoadEncodeIdx": (False, 20., 1), - "liveTracks": (True, 20.), + "radarTracks": (True, 20.), "sendcan": (True, 100., 139, QueueSize.MEDIUM), "logMessage": (True, 0., None, QueueSize.BIG), "errorLogMessage": (True, 0., 1, QueueSize.BIG), - "liveCalibration": (True, 4., 4), - "liveTorqueParameters": (True, 4., 1), - "liveDelay": (True, 4., 1), + "extrinsicsCalibration": (True, 4., 4), + "lateralTorqueParameters": (True, 4., 1), + "lateralDelay": (True, 4., 1), "operatingSystemLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), @@ -55,8 +55,8 @@ _services: dict[str, tuple] = { "qcomGnss": (True, 2.), "clocks": (True, 0.1, 1), "ubloxRaw": (True, 20.), - "livePose": (True, 20., 4), - "liveParameters": (True, 20., 5), + "deviceMotion": (True, 20., 4), + "vehicleParameters": (True, 20., 5), "cameraOdometry": (True, 20., 10), "thumbnail": (True, 1 / 60., 1), "onroadEvents": (True, 1., 1), diff --git a/openpilot/common/mock/__init__.py b/openpilot/common/mock/__init__.py index ff4dd32b92..9fa1e4b4d7 100644 --- a/openpilot/common/mock/__init__.py +++ b/openpilot/common/mock/__init__.py @@ -8,12 +8,12 @@ import functools import threading from openpilot.cereal.messaging import PubMaster from openpilot.cereal.services import SERVICE_LIST -from openpilot.common.mock.generators import generate_livePose +from openpilot.common.mock.generators import generate_deviceMotion from openpilot.common.realtime import Ratekeeper MOCK_GENERATOR = { - "livePose": generate_livePose + "deviceMotion": generate_deviceMotion } diff --git a/openpilot/common/mock/generators.py b/openpilot/common/mock/generators.py index 28a3b98e58..176471f1ab 100644 --- a/openpilot/common/mock/generators.py +++ b/openpilot/common/mock/generators.py @@ -1,14 +1,14 @@ from openpilot.cereal import messaging -def generate_livePose(): - msg = messaging.new_message('livePose') +def generate_deviceMotion(): + msg = messaging.new_message('deviceMotion') 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 + msg.deviceMotion.orientationNED = meas + msg.deviceMotion.velocityDevice = meas + msg.deviceMotion.angularVelocityDevice = meas + msg.deviceMotion.accelerationDevice = meas + msg.deviceMotion.inputsOK = True + msg.deviceMotion.posenetOK = True + msg.deviceMotion.sensorsOK = True return msg diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 5bdfdf8636..0019f6c9ac 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -77,6 +77,7 @@ inline static std::unordered_map keys = { {"LastUpdateRouteCount", {PERSISTENT, INT, "0"}}, {"LastUpdateTime", {PERSISTENT, TIME}}, {"LastUpdateUptimeOnroad", {PERSISTENT, FLOAT, "0.0"}}, + // TODO: rename the Live* learner cache keys to match their Cereal services, with migration for persisted values. {"LiveDelay", {PERSISTENT, BYTES}}, {"LiveParametersV2", {PERSISTENT, BYTES}}, {"LivestreamEncoderBitrate", {CLEAR_ON_MANAGER_START | DONT_LOG, INT}}, diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 96c169a1da..86f8fbbde5 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -66,7 +66,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'radarTracks']) self.can_rcv_cum_timeout_counter = 0 @@ -216,10 +216,10 @@ class Car: self.pm.send('carState', cs_send) if RD is not None: - tracks_msg = messaging.new_message('liveTracks') + tracks_msg = messaging.new_message('radarTracks') tracks_msg.valid = not any(RD.errors.to_dict().values()) - tracks_msg.liveTracks = RD - self.pm.send('liveTracks', tracks_msg) + tracks_msg.radarTracks = RD + self.pm.send('radarTracks', tracks_msg) def controls_update(self, CS: car.CarState, CC: car.CarControl): """control update loop, driven by carControl""" diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 9afcdf1d27..aef92fb7ab 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -38,8 +38,8 @@ class Controls: self.CI = interfaces[self.CP.carFingerprint](self.CP) - self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', + self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState', + 'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) @@ -64,17 +64,17 @@ 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) + if self.sm.updated["extrinsicsCalibration"]: + self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration']) + if self.sm.updated["deviceMotion"]: + device_motion = Pose.from_device_motion(self.sm['deviceMotion']) + self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion) def state_control(self): CS = self.sm['carState'] # Update VehicleModel - lp = self.sm['liveParameters'] + lp = self.sm['vehicleParameters'] x = max(lp.stiffnessFactor, 0.1) sr = max(lp.steerRatio, 0.1) self.VM.update_params(x, sr) @@ -84,9 +84,9 @@ class Controls: # Update Torque Params if self.CP.lateralTuning.which() == 'torque': - torque_params = self.sm['liveTorqueParameters'] - if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams: - self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, + torque_params = self.sm['lateralTorqueParameters'] + if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams: + self.LaC.update_torque_parameters(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) long_plan = self.sm['longitudinalPlan'] @@ -125,7 +125,7 @@ class Controls: else: new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) - lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS actuators.curvature = self.desired_curvature steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, diff --git a/openpilot/selfdrive/controls/lib/latcontrol_torque.py b/openpilot/selfdrive/controls/lib/latcontrol_torque.py index 846f1daf00..267c5b5967 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_torque.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_torque.py @@ -46,7 +46,7 @@ class LatControlTorque(LatControl): self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) - def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): + def update_torque_parameters(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 725491eed2..0a0722fbc6 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -102,7 +102,7 @@ def gen_long_model(): a_ego_dot = SX.sym('a_ego_dot') model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) - # live parameters + # runtime parameters a_min = SX.sym('a_min') a_max = SX.sym('a_max') x_obstacle = SX.sym('x_obstacle') diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 8855d85af4..c2b8b94abb 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -96,7 +96,7 @@ class LongitudinalPlanner: throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0 self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg if reset_state: self.v_desired_filter.x = v_ego diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index 60a6525853..110c124166 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/openpilot/selfdrive/controls/plannerd.py @@ -19,7 +19,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2') while True: diff --git a/openpilot/selfdrive/controls/radard.py b/openpilot/selfdrive/controls/radard.py index 6fecfffb1d..e6b5a0aea8 100755 --- a/openpilot/selfdrive/controls/radard.py +++ b/openpilot/selfdrive/controls/radard.py @@ -260,7 +260,7 @@ def main() -> None: cloudlog.info("radard got CarParams") # *** setup messaging - sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') + sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2') pm = messaging.PubMaster(['radarState']) RD = RadarD(CP.radarDelay) @@ -268,7 +268,7 @@ def main() -> None: while 1: sm.update() - RD.update(sm, sm['liveTracks']) + RD.update(sm, sm['radarTracks']) RD.publish(pm) diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol.py b/openpilot/selfdrive/controls/tests/test_latcontrol.py index 7f2ab8d6b7..2b7a0cf695 100644 --- a/openpilot/selfdrive/controls/tests/test_latcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_latcontrol.py @@ -31,7 +31,7 @@ class TestLatControl(OpenpilotTestCase): CS.vEgo = 30 CS.steeringPressed = False - params = log.LiveParametersData.new_message() + params = log.VehicleParameters.new_message() # Saturate for curvature limited and controller limited for _ in range(1000): diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py b/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py index 65befdec0f..6be242821c 100644 --- a/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py +++ b/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py @@ -27,7 +27,7 @@ class TestLatControlTorqueBuffer(OpenpilotTestCase): CS = car.CarState.new_message() CS.vEgo = 30 CS.steeringPressed = False - params = log.LiveParametersData.new_message() + params = log.VehicleParameters.new_message() for _ in range(buffer_steps): controller.update(True, CS, VM, params, False, 0.001, False, 0.2) diff --git a/openpilot/selfdrive/controls/tests/test_leads.py b/openpilot/selfdrive/controls/tests/test_leads.py index a3e226cb4b..45257c545b 100644 --- a/openpilot/selfdrive/controls/tests/test_leads.py +++ b/openpilot/selfdrive/controls/tests/test_leads.py @@ -26,7 +26,7 @@ class TestLeads(OpenpilotTestCase): msgs = [m for _ in range(3) for m in single_iter_pkg()] out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2) - states = [m for m in out if m.which() == "liveTracks"] + states = [m for m in out if m.which() == "radarTracks"] failures = [not state.valid for state in states] assert len(states) == 0 or all(failures) diff --git a/openpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/openpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py index 4fe855544e..011713dfb3 100644 --- a/openpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py +++ b/openpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py @@ -41,7 +41,7 @@ def simulate_straight_road_msgs(est): carControl = messaging.new_message('carControl').carControl carOutput = messaging.new_message('carOutput').carOutput carState = messaging.new_message('carState').carState - livePose = messaging.new_message('livePose').livePose + deviceMotion = messaging.new_message('deviceMotion').deviceMotion carControl.latActive = True carState.vEgo = V_EGO carState.steeringPressed = False @@ -50,11 +50,11 @@ def simulate_straight_road_msgs(est): lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True): carOutput.actuatorsOutput.torque = float(-steer_torque) - livePose.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True} - livePose.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True} - livePose.inputsOK, livePose.sensorsOK, livePose.posenetOK = True, True, True - livePose.timestamp = int(t * 1e9) - for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)): + deviceMotion.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True} + deviceMotion.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True} + deviceMotion.inputsOK, deviceMotion.sensorsOK, deviceMotion.posenetOK = True, True, True + deviceMotion.timestamp = int(t * 1e9) + for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('deviceMotion', deviceMotion)): est.handle_log(t, which, msg) class TestTorquedLatAccelOffset(OpenpilotTestCase): @@ -63,11 +63,11 @@ class TestTorquedLatAccelOffset(OpenpilotTestCase): est = get_warmed_up_estimator(steer_torques, lat_accels) msg = est.get_msg() # TODO add lataccelfactor and friction check when we have more accurate estimates - assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1 + assert abs(msg.lateralTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1 def test_straight_road_roll_bias(self): steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) est = get_warmed_up_estimator(steer_torques, lat_accels) simulate_straight_road_msgs(est) msg = est.get_msg() - assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw) + assert (msg.lateralTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.lateralTorqueParameters.latAccelOffsetRaw) diff --git a/openpilot/selfdrive/locationd/calibrationd.py b/openpilot/selfdrive/locationd/calibrationd.py index b9616bf632..ec9a225634 100755 --- a/openpilot/selfdrive/locationd/calibrationd.py +++ b/openpilot/selfdrive/locationd/calibrationd.py @@ -74,15 +74,15 @@ class Calibrator: wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT height = HEIGHT_INIT valid_blocks = 0 - self.cal_status = log.LiveCalibrationData.Status.uncalibrated + self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated if param_put and calibration_params: try: with log.Event.from_bytes(calibration_params) as msg: - rpy_init = np.array(msg.liveCalibration.rpyCalib) - valid_blocks = msg.liveCalibration.validBlocks - wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler) - height = np.array(msg.liveCalibration.height) + rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib) + valid_blocks = msg.extrinsicsCalibration.validBlocks + wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler) + height = np.array(msg.extrinsicsCalibration.height) except Exception: cloudlog.exception("Error reading cached CalibrationParams") @@ -149,22 +149,22 @@ class Calibrator: self.calib_spread = np.zeros(3) if self.valid_blocks < INPUTS_NEEDED: - if self.cal_status == log.LiveCalibrationData.Status.recalibrating: - self.cal_status = log.LiveCalibrationData.Status.recalibrating + if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating: + self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating else: - self.cal_status = log.LiveCalibrationData.Status.uncalibrated + self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated elif is_calibration_valid(self.rpy): - self.cal_status = log.LiveCalibrationData.Status.calibrated + self.cal_status = log.ExtrinsicsCalibration.Status.calibrated else: - self.cal_status = log.LiveCalibrationData.Status.invalid + self.cal_status = log.ExtrinsicsCalibration.Status.invalid # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. # TODO: add height spread check with smooth transition too spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD - if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated: + if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated: self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) - self.cal_status = log.LiveCalibrationData.Status.recalibrating + self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: @@ -234,35 +234,35 @@ class Calibrator: def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder: smooth_rpy = self.get_smooth_rpy() - msg = messaging.new_message('liveCalibration') + msg = messaging.new_message('extrinsicsCalibration') msg.valid = valid - liveCalibration = msg.liveCalibration - liveCalibration.validBlocks = self.valid_blocks - liveCalibration.calStatus = self.cal_status - liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) - liveCalibration.rpyCalib = smooth_rpy.tolist() - liveCalibration.rpyCalibSpread = self.calib_spread.tolist() - liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() - liveCalibration.height = self.height.tolist() + extrinsicsCalibration = msg.extrinsicsCalibration + extrinsicsCalibration.validBlocks = self.valid_blocks + extrinsicsCalibration.calStatus = self.cal_status + extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) + extrinsicsCalibration.rpyCalib = smooth_rpy.tolist() + extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist() + extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() + extrinsicsCalibration.height = self.height.tolist() if self.not_car: - liveCalibration.validBlocks = INPUTS_NEEDED - liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated - liveCalibration.calPerc = 100. - liveCalibration.rpyCalib = [0, 0, 0] - liveCalibration.rpyCalibSpread = self.calib_spread.tolist() + extrinsicsCalibration.validBlocks = INPUTS_NEEDED + extrinsicsCalibration.calStatus = log.ExtrinsicsCalibration.Status.calibrated + extrinsicsCalibration.calPerc = 100. + extrinsicsCalibration.rpyCalib = [0, 0, 0] + extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist() return msg def send_data(self, pm: messaging.PubMaster, valid: bool) -> None: - pm.send('liveCalibration', self.get_msg(valid)) + pm.send('extrinsicsCalibration', self.get_msg(valid)) def main() -> NoReturn: config_realtime_process([0, 1, 2, 3], 5) - pm = messaging.PubMaster(['liveCalibration']) + pm = messaging.PubMaster(['extrinsicsCalibration']) sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry') params_reader = Params() diff --git a/openpilot/selfdrive/locationd/helpers.py b/openpilot/selfdrive/locationd/helpers.py index 9ea9b6bf4f..48e1ee25c2 100644 --- a/openpilot/selfdrive/locationd/helpers.py +++ b/openpilot/selfdrive/locationd/helpers.py @@ -130,7 +130,7 @@ class Measurement: self.xyz_std: np.ndarray = xyz_std @classmethod - def from_measurement_xyz(cls, measurement: log.LivePose.XYZMeasurement) -> 'Measurement': + def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement': return cls( xyz=np.array([measurement.x, measurement.y, measurement.z]), xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd]) @@ -145,12 +145,12 @@ class Pose: self.angular_velocity = angular_velocity @classmethod - def from_live_pose(cls, live_pose: log.LivePose) -> 'Pose': + def from_device_motion(cls, device_motion: log.DeviceMotion) -> 'Pose': return Pose( - orientation=Measurement.from_measurement_xyz(live_pose.orientationNED), - velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice), - acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice), - angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice) + orientation=Measurement.from_measurement_xyz(device_motion.orientationNED), + velocity=Measurement.from_measurement_xyz(device_motion.velocityDevice), + acceleration=Measurement.from_measurement_xyz(device_motion.accelerationDevice), + angular_velocity=Measurement.from_measurement_xyz(device_motion.angularVelocityDevice) ) @@ -178,8 +178,8 @@ class PoseCalibrator: return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib) - def feed_live_calib(self, live_calib: log.LiveCalibrationData): - calib_rpy = np.array(live_calib.rpyCalib) + def feed_extrinsics_calibration(self, extrinsics_calibration: log.ExtrinsicsCalibration): + calib_rpy = np.array(extrinsics_calibration.rpyCalib) device_from_calib = rot_from_euler(calib_rpy) self.calib_from_device = device_from_calib.T - self.calib_valid = live_calib.calStatus == log.LiveCalibrationData.Status.calibrated + self.calib_valid = extrinsics_calibration.calStatus == log.ExtrinsicsCalibration.Status.calibrated diff --git a/openpilot/selfdrive/locationd/lagd.py b/openpilot/selfdrive/locationd/lagd.py index d3c0c5195b..a198ce915f 100755 --- a/openpilot/selfdrive/locationd/lagd.py +++ b/openpilot/selfdrive/locationd/lagd.py @@ -171,7 +171,7 @@ class BlockAverage: class LateralLagEstimator: - inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"} + inputs = {"carControl", "carState", "controlsState", "extrinsicsCalibration", "deviceMotion"} 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, @@ -219,39 +219,39 @@ class LateralLagEstimator: self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: - msg = messaging.new_message('liveDelay') + msg = messaging.new_message('lateralDelay') msg.valid = valid - liveDelay = msg.liveDelay + lateralDelay = msg.lateralDelay valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): if valid_std > MAX_LAG_STD: - liveDelay.status = log.LiveDelayData.Status.invalid + lateralDelay.status = log.LateralDelay.Status.invalid else: - liveDelay.status = log.LiveDelayData.Status.estimated + lateralDelay.status = log.LateralDelay.Status.estimated else: - liveDelay.status = log.LiveDelayData.Status.unestimated + lateralDelay.status = log.LateralDelay.Status.unestimated - if liveDelay.status == log.LiveDelayData.Status.estimated: - liveDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag)) + if lateralDelay.status == log.LateralDelay.Status.estimated: + lateralDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag)) else: - liveDelay.lateralDelay = self.initial_lag + lateralDelay.lateralDelay = self.initial_lag if not np.isnan(current_mean_lag) and not np.isnan(current_std): - liveDelay.lateralDelayEstimate = current_mean_lag - liveDelay.lateralDelayEstimateStd = current_std + lateralDelay.lateralDelayEstimate = current_mean_lag + lateralDelay.lateralDelayEstimateStd = current_std else: - liveDelay.lateralDelayEstimate = self.initial_lag - liveDelay.lateralDelayEstimateStd = 0.0 + lateralDelay.lateralDelayEstimate = self.initial_lag + lateralDelay.lateralDelayEstimateStd = 0.0 - liveDelay.validBlocks = self.block_avg.valid_blocks - liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // + lateralDelay.validBlocks = self.block_avg.valid_blocks + lateralDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // (self.min_valid_block_count * self.block_size), 100) if debug: - liveDelay.points = self.block_avg.values.flatten().tolist() - liveDelay.version = VERSION + lateralDelay.points = self.block_avg.values.flatten().tolist() + lateralDelay.version = VERSION return msg @@ -264,11 +264,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) + elif which == "extrinsicsCalibration": + self.calibrator.feed_extrinsics_calibration(msg) + elif which == "deviceMotion": + device_motion = Pose.from_device_motion(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_motion) 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 @@ -368,13 +368,13 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams): if last_lag_data is not None: try: with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: - ld = last_lag_msg.liveDelay + ld = last_lag_msg.lateralDelay if last_CP.carFingerprint != CP.carFingerprint: raise Exception("Car model mismatch") lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" - assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid" + assert status != log.LateralDelay.Status.invalid, "Lag estimate is invalid" assert version == VERSION, f"Lag estimate is from a different version (got {version}, expected {VERSION})" return lag, valid_blocks except Exception as e: @@ -389,13 +389,13 @@ def main(): DEBUG = bool(int(os.getenv("DEBUG", "0"))) - pm = messaging.PubMaster(['liveDelay']) - sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') + pm = messaging.PubMaster(['lateralDelay']) + sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl'], poll='deviceMotion') params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) + lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency) if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) @@ -409,12 +409,12 @@ def main(): lag_learner.handle_log(t, which, sm[which]) lag_learner.update_points() - # 4Hz driven by livePose + # 4Hz driven by deviceMotion if sm.frame % 5 == 0: lag_learner.update_estimate() lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) lag_msg_dat = lag_msg.to_bytes() - pm.send('liveDelay', lag_msg_dat) + pm.send('lateralDelay', lag_msg_dat) if sm.frame % 1200 == 0: # cache every 60 seconds params.put("LiveDelay", lag_msg_dat) diff --git a/openpilot/selfdrive/locationd/locationd.py b/openpilot/selfdrive/locationd/locationd.py index eb3c42fce2..9fbec991d5 100755 --- a/openpilot/selfdrive/locationd/locationd.py +++ b/openpilot/selfdrive/locationd/locationd.py @@ -148,7 +148,7 @@ class LocationEstimator: elif which == "carState": self.car_speed = abs(msg.vEgo) - elif which == "liveCalibration": + elif which == "extrinsicsCalibration": # Note that we use this message during calibration if len(msg.rpyCalib) > 0: calib = np.array(msg.rpyCalib) @@ -217,19 +217,19 @@ class LocationEstimator: angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY] acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION] - msg = messaging.new_message("livePose") + msg = messaging.new_message("deviceMotion") msg.valid = filter_valid - livePose = msg.livePose - init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid) - init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid) - init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) - init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) + deviceMotion = msg.deviceMotion + init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid) + init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid) + init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) + init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) if self.debug: - livePose.debugFilterState.value = state.tolist() - livePose.debugFilterState.std = std.tolist() - livePose.debugFilterState.valid = filter_valid - livePose.debugFilterState.observations = [ + deviceMotion.debugFilterState.value = state.tolist() + deviceMotion.debugFilterState.std = std.tolist() + deviceMotion.debugFilterState.valid = filter_valid + deviceMotion.debugFilterState.observations = [ {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()} for k in self.observations.keys() ] @@ -238,10 +238,10 @@ class LocationEstimator: new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:]) std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0 - livePose.inputsOK = inputs_valid - livePose.posenetOK = not std_spike or self.car_speed <= 5.0 - livePose.sensorsOK = sensors_valid - livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9) + deviceMotion.inputsOK = inputs_valid + deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0 + deviceMotion.sensorsOK = sensors_valid + deviceMotion.timestamp = int(np.nan_to_num(self.kf.t) * 1e9) return msg @@ -267,8 +267,8 @@ def main(): DEBUG = bool(int(os.getenv("DEBUG", "0"))) SIMULATION = bool(int(os.getenv("SIMULATION", "0"))) - pm = messaging.PubMaster(['livePose']) - sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry') + pm = messaging.PubMaster(['deviceMotion']) + sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry') # separate sensor sockets for efficiency sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']] sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float) @@ -288,7 +288,7 @@ def main(): initial_pose_data = params.get("LocationFilterInitialState") if initial_pose_data is not None: with log.Event.from_bytes(initial_pose_data) as lp_msg: - filter_state = lp_msg.livePose.debugFilterState + filter_state = lp_msg.deviceMotion.debugFilterState x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P estimator.reset(None, x_initial, P_initial) @@ -333,7 +333,7 @@ def main(): sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) - pm.send("livePose", msg) + pm.send("deviceMotion", msg) if __name__ == "__main__": diff --git a/openpilot/selfdrive/locationd/paramsd.py b/openpilot/selfdrive/locationd/paramsd.py index 9b5ff7143e..760176bc5c 100755 --- a/openpilot/selfdrive/locationd/paramsd.py +++ b/openpilot/selfdrive/locationd/paramsd.py @@ -65,10 +65,10 @@ class VehicleParamsLearner: self.avg_angle_offset = self.angle_offset def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): - if which == 'livePose': + if which == 'deviceMotion': t = msg.timestamp * 1e-9 - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + device_motion = Pose.from_device_motion(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_motion) yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std yaw_rate_valid = msg.angularVelocityDevice.valid @@ -79,7 +79,7 @@ class VehicleParamsLearner: 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, localizer_roll_std = device_motion.orientation.x, device_motion.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: @@ -113,8 +113,8 @@ 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 == 'extrinsicsCalibration': + self.calibrator.feed_extrinsics_calibration(msg) elif which == 'carState': steering_angle = msg.steeringAngleDeg @@ -136,7 +136,7 @@ class VehicleParamsLearner: 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") + cloudlog.error("NaN in vehicleParameters estimate. Resetting to default values") self.reset(self.kf.t) x = self.kf.x @@ -156,38 +156,38 @@ class VehicleParamsLearner: 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 = messaging.new_message('vehicleParameters') 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 , + vehicleParameters = msg.vehicleParameters + vehicleParameters.posenetValid = True + vehicleParameters.sensorValid = sensors_valid + vehicleParameters.steerRatio = float(x[States.STEER_RATIO].item()) + vehicleParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) + vehicleParameters.roll = float(self.roll) + vehicleParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) + vehicleParameters.angleOffsetDeg = float(self.angle_offset) + vehicleParameters.steerRatioValid = self.min_sr <= vehicleParameters.steerRatio <= self.max_sr + vehicleParameters.stiffnessFactorValid = 0.2 <= vehicleParameters.stiffnessFactor <= 5.0 + vehicleParameters.angleOffsetAverageValid = bool(self.avg_offset_valid) + vehicleParameters.angleOffsetValid = bool(self.total_offset_valid) + vehicleParameters.valid = all(( + vehicleParameters.angleOffsetAverageValid, + vehicleParameters.angleOffsetValid , self.roll_valid, roll_std < ROLL_STD_MAX, - liveParameters.stiffnessFactorValid, - liveParameters.steerRatioValid, + vehicleParameters.stiffnessFactorValid, + vehicleParameters.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()) + vehicleParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) + vehicleParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) + vehicleParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) + vehicleParameters.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() + vehicleParameters.debugFilterState = log.VehicleParameters.FilterState.new_message() + vehicleParameters.debugFilterState.value = x.tolist() + vehicleParameters.debugFilterState.std = P.tolist() return msg @@ -210,7 +210,7 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b 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 + lp = last_lp_msg.vehicleParameters # Check if car model matches if last_CP.carFingerprint != CP.carFingerprint: raise Exception("Car model mismatch") @@ -248,8 +248,8 @@ def main(): DEBUG = bool(int(os.getenv("DEBUG", "0"))) REPLAY = bool(int(os.getenv("REPLAY", "0"))) - pm = messaging.PubMaster(['liveParameters']) - sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose') + pm = messaging.PubMaster(['vehicleParameters']) + sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState'], poll='deviceMotion') params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) @@ -265,14 +265,14 @@ def main(): t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) - if sm.updated['livePose']: + if sm.updated['deviceMotion']: msg = learner.get_msg(sm.all_checks(), debug=DEBUG) msg_dat = msg.to_bytes() if sm.frame % 1200 == 0: # once a minute params.put("LiveParametersV2", msg_dat) - pm.send('liveParameters', msg_dat) + pm.send('vehicleParameters', msg_dat) if __name__ == "__main__": diff --git a/openpilot/selfdrive/locationd/test/test_calibrationd.py b/openpilot/selfdrive/locationd/test/test_calibrationd.py index b3722e344c..7afd43b35b 100644 --- a/openpilot/selfdrive/locationd/test/test_calibrationd.py +++ b/openpilot/selfdrive/locationd/test/test_calibrationd.py @@ -33,16 +33,16 @@ def process_messages(c, cam_odo_calib, cycles, class TestCalibrationd(OpenpilotTestCase): def test_read_saved_params(self): - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = random.randint(1, 10) - msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)] - msg.liveCalibration.height = [random.random() for _ in range(1)] + msg = messaging.new_message('extrinsicsCalibration') + msg.extrinsicsCalibration.validBlocks = random.randint(1, 10) + msg.extrinsicsCalibration.rpyCalib = [random.random() for _ in range(3)] + msg.extrinsicsCalibration.height = [random.random() for _ in range(1)] Params().put("CalibrationParams", msg.to_bytes(), block=True) c = Calibrator(param_put=True) - np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) - np.testing.assert_allclose(msg.liveCalibration.height, c.height) - assert msg.liveCalibration.validBlocks == c.valid_blocks + np.testing.assert_allclose(msg.extrinsicsCalibration.rpyCalib, c.rpy) + np.testing.assert_allclose(msg.extrinsicsCalibration.height, c.height) + assert msg.extrinsicsCalibration.validBlocks == c.valid_blocks def test_calibration_basics(self): @@ -92,7 +92,7 @@ class TestCalibrationd(OpenpilotTestCase): np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3) process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10) assert c.valid_blocks == INPUTS_NEEDED + 1 - assert c.cal_status == log.LiveCalibrationData.Status.calibrated + assert c.cal_status == log.ExtrinsicsCalibration.Status.calibrated c = Calibrator(param_put=False) process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) @@ -100,7 +100,7 @@ class TestCalibrationd(OpenpilotTestCase): np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10) assert c.valid_blocks == 1 - assert c.cal_status == log.LiveCalibrationData.Status.recalibrating + assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2) c = Calibrator(param_put=False) @@ -109,5 +109,5 @@ class TestCalibrationd(OpenpilotTestCase): np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10) assert c.valid_blocks == 1 - assert c.cal_status == log.LiveCalibrationData.Status.recalibrating + assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2) diff --git a/openpilot/selfdrive/locationd/test/test_lagd.py b/openpilot/selfdrive/locationd/test/test_lagd.py index d6006c6cf9..0acc3e646f 100644 --- a/openpilot/selfdrive/locationd/test/test_lagd.py +++ b/openpilot/selfdrive/locationd/test/test_lagd.py @@ -43,9 +43,9 @@ def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_thres (t, "carControl", car.CarControl(latActive=not rejected)), (t, "carState", car.CarState(vEgo=vego, steeringPressed=False)), (t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)), - (t, "livePose", log.LivePose(angularVelocityDevice=log.LivePose.XYZMeasurement(z=actual_yr, valid=True), + (t, "deviceMotion", log.DeviceMotion(angularVelocityDevice=log.DeviceMotion.XYZMeasurement(z=actual_yr, valid=True), posenetOK=True, inputsOK=True)), - (t, "liveCalibration", log.LiveCalibrationData(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), + (t, "extrinsicsCalibration", log.ExtrinsicsCalibration(rpyCalib=[0, 0, 0], calStatus=log.ExtrinsicsCalibration.Status.calibrated)), ] for t, w, m in msgs: estimator.handle_log(t, w, m) @@ -59,10 +59,10 @@ class TestLagd(OpenpilotTestCase): CP = get_test_car_params() - msg = messaging.new_message('liveDelay') - msg.liveDelay.lateralDelayEstimate = random.random() - msg.liveDelay.validBlocks = random.randint(1, 10) - msg.liveDelay.version = VERSION + msg = messaging.new_message('lateralDelay') + msg.lateralDelay.lateralDelayEstimate = random.random() + msg.lateralDelay.validBlocks = random.randint(1, 10) + msg.lateralDelay.version = VERSION params.put("LiveDelay", msg.to_bytes(), block=True) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True) @@ -70,8 +70,8 @@ class TestLagd(OpenpilotTestCase): assert saved_lag_params is not None lag, valid_blocks = saved_lag_params - assert lag == msg.liveDelay.lateralDelayEstimate - assert valid_blocks == msg.liveDelay.validBlocks + assert lag == msg.lateralDelay.lateralDelayEstimate + assert valid_blocks == msg.lateralDelay.validBlocks def test_read_invalid_saved_params(self, subtests): params = Params() @@ -79,9 +79,9 @@ class TestLagd(OpenpilotTestCase): CP = get_test_car_params() for msg_dict in [{'version': 0}, {'status': 'invalid'}, {'validBlocks': 100}]: - with subtests.test(msg=f"liveDelay={msg_dict}"): - msg = messaging.new_message('liveDelay') - msg.liveDelay = msg_dict + with subtests.test(msg=f"lateralDelay={msg_dict}"): + msg = messaging.new_message('lateralDelay') + msg.lateralDelay = msg_dict params.put("LiveDelay", msg.to_bytes(), block=True) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True) assert retrieve_initial_lag(params, CP) is None @@ -114,11 +114,11 @@ class TestLagd(OpenpilotTestCase): mocked_CP = car.CarParams(steerActuatorDelay=0.5) estimator = LateralLagEstimator(mocked_CP, DT) msg = estimator.get_msg(True) - assert msg.liveDelay.status == 'unestimated' - assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag) - assert msg.liveDelay.validBlocks == 0 - assert msg.liveDelay.calPerc == 0 + assert msg.lateralDelay.status == 'unestimated' + assert np.allclose(msg.lateralDelay.lateralDelay, estimator.initial_lag) + assert np.allclose(msg.lateralDelay.lateralDelayEstimate, estimator.initial_lag) + assert msg.lateralDelay.validBlocks == 0 + assert msg.lateralDelay.calPerc == 0 def test_estimator_basics(self, subtests): for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1): @@ -127,21 +127,21 @@ class TestLagd(OpenpilotTestCase): estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0) process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) msg = estimator.get_msg(True) - assert msg.liveDelay.status == 'estimated' - assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) - assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED - assert msg.liveDelay.calPerc == 100 + assert msg.lateralDelay.status == 'estimated' + assert np.allclose(msg.lateralDelay.lateralDelay, lag_frames * DT, atol=0.01) + assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) + assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01) + assert msg.lateralDelay.validBlocks == BLOCK_NUM_NEEDED + assert msg.lateralDelay.calPerc == 100 def test_estimator_masking(self): mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.5), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1) estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1) process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4) msg = estimator.get_msg(True) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) - assert msg.liveDelay.calPerc == 100 + assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) + assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01) + assert msg.lateralDelay.calPerc == 100 @unittest.skipIf(PC, "only on device") def test_estimator_performance(self): diff --git a/openpilot/selfdrive/locationd/test/test_locationd_scenarios.py b/openpilot/selfdrive/locationd/test/test_locationd_scenarios.py index c5fe1908bc..01fc3f1177 100644 --- a/openpilot/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/openpilot/selfdrive/locationd/test/test_locationd_scenarios.py @@ -45,7 +45,7 @@ def get_select_fields_data(logs): for key in keys: val = getattr(val, key) if isinstance(key, str) else val[key] return val - lp = [x.livePose for x in logs if x.which() == 'livePose'] + lp = [x.deviceMotion for x in logs if x.which() == 'deviceMotion'] data = defaultdict(list) for msg in lp: for key, fields in SELECT_COMPARE_FIELDS.items(): diff --git a/openpilot/selfdrive/locationd/test/test_paramsd.py b/openpilot/selfdrive/locationd/test/test_paramsd.py index 09a067f4cf..515c7a814a 100644 --- a/openpilot/selfdrive/locationd/test/test_paramsd.py +++ b/openpilot/selfdrive/locationd/test/test_paramsd.py @@ -11,12 +11,12 @@ from openpilot.common.params import Params from openpilot.tools.lib.logreader import LogReader -def get_random_live_parameters(CP): - msg = messaging.new_message("liveParameters") - msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio - msg.liveParameters.stiffnessFactor = random.random() - msg.liveParameters.angleOffsetAverageDeg = random.random() - msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])] +def get_random_vehicle_parameters(CP): + msg = messaging.new_message("vehicleParameters") + msg.vehicleParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio + msg.vehicleParameters.stiffnessFactor = random.random() + msg.vehicleParameters.angleOffsetAverageDeg = random.random() + msg.vehicleParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])] return msg @@ -27,13 +27,13 @@ class TestParamsd(OpenpilotTestCase): lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) CP = next(m for m in lr if m.which() == "carParams").carParams - msg = get_random_live_parameters(CP) + msg = get_random_vehicle_parameters(CP) params.put("LiveParametersV2", msg.to_bytes(), block=True) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True) sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True) - np.testing.assert_allclose(sr, msg.liveParameters.steerRatio) - np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor) - np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg) + np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio) + np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor) + np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg) np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape) - np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std) + np.testing.assert_allclose(np.diagonal(p_init), msg.vehicleParameters.debugFilterState.std) diff --git a/openpilot/selfdrive/locationd/test/test_torqued.py b/openpilot/selfdrive/locationd/test/test_torqued.py index af3aeb95d6..3c5cb29bfc 100644 --- a/openpilot/selfdrive/locationd/test/test_torqued.py +++ b/openpilot/selfdrive/locationd/test/test_torqued.py @@ -7,7 +7,7 @@ class TestTorqued(OpenpilotTestCase): def test_cal_percent(self): est = TorqueEstimator(car.CarParams()) msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == 0 + assert msg.lateralTorqueParameters.calPerc == 0 for (low, high), min_pts in zip(est.filtered_points.buckets.keys(), est.filtered_points.buckets_min_points.values(), strict=True): @@ -16,7 +16,7 @@ class TestTorqued(OpenpilotTestCase): # enough bucket points, but not enough total points msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2 + assert msg.lateralTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2 # add enough points to bucket with most capacity key = list(est.filtered_points.buckets)[0] @@ -24,4 +24,4 @@ class TestTorqued(OpenpilotTestCase): est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0) msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == 100 + assert msg.lateralTorqueParameters.calPerc == 100 diff --git a/openpilot/selfdrive/locationd/torqued.py b/openpilot/selfdrive/locationd/torqued.py index d36684563c..21b2af7f12 100755 --- a/openpilot/selfdrive/locationd/torqued.py +++ b/openpilot/selfdrive/locationd/torqued.py @@ -102,11 +102,11 @@ class TorqueEstimator(ParameterEstimator): if params_cache is not None and torque_cache is not None: try: with log.Event.from_bytes(torque_cache) as log_evt: - cache_ltp = log_evt.liveTorqueParameters + cache_ltp = log_evt.lateralTorqueParameters with car.CarParams.from_bytes(params_cache) as msg: cache_CP = msg if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION): - if cache_ltp.liveValid: + if cache_ltp.valid: initial_params = { 'latAccelFactor': cache_ltp.latAccelFactorFiltered, 'latAccelOffset': cache_ltp.latAccelOffsetFiltered, @@ -154,7 +154,7 @@ class TorqueEstimator(ParameterEstimator): _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T friction_coeff = np.std(spread) * FRICTION_FACTOR except np.linalg.LinAlgError as e: - cloudlog.exception(f"Error computing live torque params: {e}") + cloudlog.exception(f"Error computing lateral torque parameters: {e}") slope = offset = friction_coeff = np.nan return slope, offset, friction_coeff @@ -176,21 +176,21 @@ 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) - elif which == "liveDelay": + elif which == "extrinsicsCalibration": + self.calibrator.feed_extrinsics_calibration(msg) + elif which == "lateralDelay": self.lag = msg.lateralDelay # calculate lateral accel from past steering torque - elif which == "livePose": + elif which == "deviceMotion": is_valid = msg.angularVelocityDevice.valid and msg.orientationNED.valid and msg.inputsOK and msg.sensorsOK and msg.posenetOK if len(self.raw_points['steer_torque']) == self.hist_len and is_valid: t = msg.timestamp * 1e-9 - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + device_motion = Pose.from_device_motion(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_motion) angular_velocity_calibrated = calibrated_pose.angular_velocity yaw_rate = angular_velocity_calibrated.yaw - roll = device_pose.orientation.roll + roll = device_motion.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) @@ -207,40 +207,40 @@ class TorqueEstimator(ParameterEstimator): self.all_torque_points.append([steer, lateral_acc]) def get_msg(self, valid=True, with_points=False): - msg = messaging.new_message('liveTorqueParameters') + msg = messaging.new_message('lateralTorqueParameters') msg.valid = valid - liveTorqueParameters = msg.liveTorqueParameters - liveTorqueParameters.version = VERSION - liveTorqueParameters.useParams = self.use_params + lateralTorqueParameters = msg.lateralTorqueParameters + lateralTorqueParameters.version = VERSION + lateralTorqueParameters.useParams = self.use_params # Calculate raw estimates when possible, only update filters when enough points are gathered if self.filtered_points.is_calculable(): latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() - liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) - liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) - liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) + lateralTorqueParameters.latAccelFactorRaw = float(latAccelFactor) + lateralTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) + lateralTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) if self.filtered_points.is_valid(): if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]): - cloudlog.exception("Live torque parameters are invalid.") - liveTorqueParameters.liveValid = False + cloudlog.exception("Lateral torque parameters are invalid.") + lateralTorqueParameters.valid = False self.reset() else: - liveTorqueParameters.liveValid = True + lateralTorqueParameters.valid = True latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor) frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction) self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff}) if with_points: - liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() + lateralTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() - liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) - liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) - liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) - liveTorqueParameters.totalBucketPoints = len(self.filtered_points) - liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent() - liveTorqueParameters.decay = self.decay - liveTorqueParameters.maxResets = self.resets + lateralTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) + lateralTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) + lateralTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) + lateralTorqueParameters.totalBucketPoints = len(self.filtered_points) + lateralTorqueParameters.calPerc = self.filtered_points.get_valid_percent() + lateralTorqueParameters.decay = self.decay + lateralTorqueParameters.maxResets = self.resets return msg @@ -249,8 +249,8 @@ def main(demo=False): DEBUG = bool(int(os.getenv("DEBUG", "0"))) - pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') + pm = messaging.PubMaster(['lateralTorqueParameters']) + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion', 'lateralDelay'], poll='deviceMotion') params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) @@ -263,9 +263,9 @@ def main(demo=False): t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) - # 4Hz driven by livePose + # 4Hz driven by deviceMotion if sm.frame % 5 == 0: - pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG)) + pm.send('lateralTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG)) # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: diff --git a/openpilot/selfdrive/modeld/dmonitoringmodeld.py b/openpilot/selfdrive/modeld/dmonitoringmodeld.py index 02391aeee3..67161c35bd 100755 --- a/openpilot/selfdrive/modeld/dmonitoringmodeld.py +++ b/openpilot/selfdrive/modeld/dmonitoringmodeld.py @@ -120,7 +120,7 @@ def main(): model = ModelState(vipc_client.width, vipc_client.height) cloudlog.warning("models loaded, dmonitoringmodeld starting") - sm = SubMaster(["liveCalibration"]) + sm = SubMaster(["extrinsicsCalibration"]) pm = PubMaster(["driverStateV2"]) calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32) @@ -136,8 +136,8 @@ def main(): model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32) sm.update(0) - if sm.updated["liveCalibration"]: - calib[:] = np.array(sm["liveCalibration"].rpyCalib) + if sm.updated["extrinsicsCalibration"]: + calib[:] = np.array(sm["extrinsicsCalibration"].rpyCalib) t1 = time.perf_counter() model_output, gpu_execution_time = model.run(buf, calib, model_transform) diff --git a/openpilot/selfdrive/modeld/fill_model_msg.py b/openpilot/selfdrive/modeld/fill_model_msg.py index d926ace3da..558f881b37 100644 --- a/openpilot/selfdrive/modeld/fill_model_msg.py +++ b/openpilot/selfdrive/modeld/fill_model_msg.py @@ -174,8 +174,8 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, modelV2.rawPredictions = net_output_data['raw_pred'].tobytes() def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], - vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: - msg.valid = live_calib_seen & (vipc_dropped_frames < 1) + vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, extrinsics_calibration_seen: bool) -> None: + msg.valid = extrinsics_calibration_seen & (vipc_dropped_frames < 1) cameraOdometry = msg.cameraOdometry cameraOdometry.frameId = vipc_frame_id diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index f724e530e2..e87d4f3111 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -262,7 +262,7 @@ def main(demo=False): # messaging pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutState"] if USBGPU else []) pm = PubMaster(pub_socks) - sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) + sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"]) publish_state = PublishState() params = Params() @@ -276,7 +276,7 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) - live_calib_seen = False + extrinsics_calibration_seen = False buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() @@ -332,16 +332,16 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["narrowRoadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS - if sm.updated["liveCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: - device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) + lat_delay = sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS + if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: + device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))] main_intrinsics = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32) has_wide_camera = use_extra_client or main_wide_camera extra_intrinsics = dc.wide_road.intrinsics if has_wide_camera else dc.narrow_road.intrinsics model_transform_extra = get_warp_matrix(device_from_calib_euler, extra_intrinsics, True).astype(np.float32) - live_calib_seen = True + extrinsics_calibration_seen = True traffic_convention = np.zeros(2) traffic_convention[int(is_rhd)] = 1 @@ -396,7 +396,7 @@ def main(demo=False): prev_action = action fill_model_msg(modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, - frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) + frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, extrinsics_calibration_seen) modelv2_send.modelV2.big = model.usbgpu desire_state = modelv2_send.modelV2.meta.desireState @@ -408,7 +408,7 @@ def main(demo=False): modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction fill_driving_model_data(drivingdata_send, modelv2_send) - fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) + fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, extrinsics_calibration_seen) pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) diff --git a/openpilot/selfdrive/monitoring/dmonitoringd.py b/openpilot/selfdrive/monitoring/dmonitoringd.py index a70659cb06..e686266fe8 100755 --- a/openpilot/selfdrive/monitoring/dmonitoringd.py +++ b/openpilot/selfdrive/monitoring/dmonitoringd.py @@ -10,7 +10,7 @@ def dmonitoringd_thread(): params = Params() pm = messaging.PubMaster(['driverMonitoringState']) - sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') + sm = messaging.SubMaster(['driverStateV2', 'extrinsicsCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) demo_mode=False diff --git a/openpilot/selfdrive/monitoring/policy.py b/openpilot/selfdrive/monitoring/policy.py index b58f1e66dc..06aca55407 100644 --- a/openpilot/selfdrive/monitoring/policy.py +++ b/openpilot/selfdrive/monitoring/policy.py @@ -441,7 +441,7 @@ class DriverMonitoring: driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s steering_angle_deg = sm['carState'].steeringAngleDeg - rpyCalib = sm['liveCalibration'].rpyCalib + rpyCalib = sm['extrinsicsCalibration'].rpyCalib self._set_pose_strictness( brake_disengage_prob=brake_disengage_prob, diff --git a/openpilot/selfdrive/selfdrived/events.py b/openpilot/selfdrive/selfdrived/events.py index 2127cdeceb..1482a4e334 100755 --- a/openpilot/selfdrive/selfdrived/events.py +++ b/openpilot/selfdrive/selfdrived/events.py @@ -255,9 +255,9 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' + first_word = 'Recalibrating' if sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.recalibrating else 'Calibrating' return Alert( - f"{first_word}: {sm['liveCalibration'].calPerc:.0f}%", + f"{first_word}: {sm['extrinsicsCalibration'].calPerc:.0f}%", f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) @@ -303,7 +303,7 @@ def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - rpy = sm['liveCalibration'].rpyCalib + rpy = sm['extrinsicsCalibration'].rpyCalib yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)" @@ -311,16 +311,16 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if not sm['liveParameters'].angleOffsetValid: - angle_offset_deg = sm['liveParameters'].angleOffsetDeg + if not sm['vehicleParameters'].angleOffsetValid: + angle_offset_deg = sm['vehicleParameters'].angleOffsetDeg title = "Steering misalignment detected" text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)" - elif not sm['liveParameters'].steerRatioValid: - steer_ratio = sm['liveParameters'].steerRatio + elif not sm['vehicleParameters'].steerRatioValid: + steer_ratio = sm['vehicleParameters'].steerRatio title = "Steer ratio mismatch" text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})" - elif not sm['liveParameters'].stiffnessFactorValid: - stiffness_factor = sm['liveParameters'].stiffnessFactor + elif not sm['vehicleParameters'].stiffnessFactorValid: + stiffness_factor = sm['vehicleParameters'].stiffnessFactor title = "Abnormal tire stiffness" text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})" else: diff --git a/openpilot/selfdrive/selfdrived/helpers.py b/openpilot/selfdrive/selfdrived/helpers.py index 3b3c44ecaf..e7dca0f9d5 100644 --- a/openpilot/selfdrive/selfdrived/helpers.py +++ b/openpilot/selfdrive/selfdrived/helpers.py @@ -31,7 +31,7 @@ class ExcessiveActuationCheck: # lateral yaw_rate = calibrated_pose.angular_velocity.yaw - roll = sm['liveParameters'].roll + roll = sm['vehicleParameters'].roll roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) # Prevent false positives after overriding @@ -41,9 +41,9 @@ class ExcessiveActuationCheck: if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2: excessive_lat_actuation = True - # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements - livepose_valid = abs(CS.aEgo - accel_calibrated) < 2 - self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0 + # deviceMotion acceleration can be noisy due to bad mounting or aliased deviceMotion measurements + device_motion_valid = abs(CS.aEgo - accel_calibrated) < 2 + self._excessive_counter = self._excessive_counter + 1 if device_motion_valid and (excessive_long_actuation or excessive_lat_actuation) else 0 excessive_type = None if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT: diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 59c5f2695e..73f382ac3c 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -88,9 +88,9 @@ class SelfdriveD: if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['narrowRoadCameraState', 'wideRoadCameraState'] - self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', - 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'extrinsicsCalibration', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'deviceMotion', 'lateralDelay', + 'managerState', 'vehicleParameters', 'radarState', 'lateralTorqueParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'lateralManeuverPlan'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, @@ -270,11 +270,11 @@ class SelfdriveD: self.last_functional_fan_frame = self.sm.frame # Handle calibration status - cal_status = self.sm['liveCalibration'].calStatus - if cal_status != log.LiveCalibrationData.Status.calibrated: - if cal_status == log.LiveCalibrationData.Status.uncalibrated: + cal_status = self.sm['extrinsicsCalibration'].calStatus + if cal_status != log.ExtrinsicsCalibration.Status.calibrated: + if cal_status == log.ExtrinsicsCalibration.Status.uncalibrated: self.events.add(EventName.calibrationIncomplete) - elif cal_status == log.LiveCalibrationData.Status.recalibrating: + elif cal_status == log.ExtrinsicsCalibration.Status.recalibrating: if not self.recalibrating_seen: set_offroad_alert("Offroad_Recalibration", True) self.recalibrating_seen = True @@ -291,11 +291,11 @@ class SelfdriveD: # NOTE: To fork maintainers. # Disabling or nerfing safety features will get you and your users banned from our servers. # We recommend that you do not change these numbers from the defaults. - 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) + if self.sm.updated['extrinsicsCalibration']: + self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration']) + if self.sm.updated['deviceMotion']: + device_motion = Pose.from_device_motion(self.sm['deviceMotion']) + self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion) if self.calibrated_pose is not None: excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose) @@ -395,11 +395,12 @@ class SelfdriveD: self.logged_comm_issue = None if not self.CP.notCar and not big_model_settling: # localization has nothing to work with during the load - if not self.sm['livePose'].posenetOK: + if not self.sm['deviceMotion'].posenetOK: self.events.add(EventName.posenetInvalid) - if not self.sm['livePose'].inputsOK: + if not self.sm['deviceMotion'].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['vehicleParameters'].valid and cal_status == log.ExtrinsicsCalibration.Status.calibrated 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 @@ -438,7 +439,7 @@ class SelfdriveD: # GPS checks 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['deviceMotion'].inputsOK and (self.distance_traveled > 1500): self.events.add(EventName.noGps) if gps_ok: self.distance_traveled = 0 diff --git a/openpilot/selfdrive/test/helpers.py b/openpilot/selfdrive/test/helpers.py index 2bcb6d8409..71cdcee5e7 100644 --- a/openpilot/selfdrive/test/helpers.py +++ b/openpilot/selfdrive/test/helpers.py @@ -22,9 +22,9 @@ def set_params_enabled(): params.put_bool("OpenpilotEnabledToggle", True, block=True) # valid calib - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = 20 - msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + msg = messaging.new_message('extrinsicsCalibration') + msg.extrinsicsCalibration.validBlocks = 20 + msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0, 0.0] params.put("CalibrationParams", msg.to_bytes(), block=True) def release_only(f): diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py index bdd5c51ee4..036aaa2ed3 100755 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -64,7 +64,7 @@ class Plant: control = messaging.new_message('controlsState') ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') - lp = messaging.new_message('liveParameters') + lp = messaging.new_message('vehicleParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts @@ -132,7 +132,7 @@ class Plant: 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, - 'liveParameters': lp.liveParameters, + 'vehicleParameters': lp.vehicleParameters, 'modelV2': model.modelV2} self.planner.update(sm) self.acceleration = self.planner.output_a_target diff --git a/openpilot/selfdrive/test/process_replay/README.md b/openpilot/selfdrive/test/process_replay/README.md index f39333a3c6..36d669dda6 100644 --- a/openpilot/selfdrive/test/process_replay/README.md +++ b/openpilot/selfdrive/test/process_replay/README.md @@ -85,7 +85,7 @@ Supported processes: * modeld * dmonitoringmodeld -Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. +Certain processes may require an initial state, which is usually supplied within `Params` and persists from segment to segment (for example `CalibrationParams` or the learner cache keys like `LiveParametersV2`). The `custom_params` is a dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. ```py from openpilot.selfdrive.test.process_replay import get_custom_params_from_lr diff --git a/openpilot/selfdrive/test/process_replay/migration.py b/openpilot/selfdrive/test/process_replay/migration.py index 65d54221ee..2fb8932abc 100644 --- a/openpilot/selfdrive/test/process_replay/migration.py +++ b/openpilot/selfdrive/test/process_replay/migration.py @@ -40,7 +40,7 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo migrate_controlsState, migrate_carState, migrate_liveLocationKalman, - migrate_livePose, + migrate_deviceMotion, migrate_liveTracks, migrate_driverAssistance, migrate_drivingModelData, @@ -161,11 +161,11 @@ def migrate_drivingModelData(msgs): return [], add_ops, [] -@migration(inputs=["liveTracksDEPRECATED"], product="liveTracks") +@migration(inputs=["liveTracksDEPRECATED"], product="radarTracks") def migrate_liveTracks(msgs): ops = [] for index, msg in msgs: - new_msg = messaging.new_message('liveTracks') + new_msg = messaging.new_message('radarTracks') new_msg.valid = msg.valid new_msg.logMonoTime = msg.logMonoTime @@ -179,42 +179,42 @@ def migrate_liveTracks(msgs): pt.vRel = track.vRel pts.append(pt) - new_msg.liveTracks.points = pts + new_msg.radarTracks.points = pts ops.append((index, as_reader(new_msg))) return ops, [], [] -@migration(inputs=["liveLocationKalmanDEPRECATED"], product="livePose") +@migration(inputs=["liveLocationKalmanDEPRECATED"], product="deviceMotion") def migrate_liveLocationKalman(msgs): nans = [float('nan')] * 3 ops = [] for index, msg in msgs: - m = messaging.new_message('livePose') + m = messaging.new_message('deviceMotion') m.valid = msg.valid m.logMonoTime = msg.logMonoTime - m.livePose.timestamp = msg.logMonoTime + m.deviceMotion.timestamp = msg.logMonoTime for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]: - lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field) + lp_field, llk_field = getattr(m.deviceMotion, field), getattr(msg.liveLocationKalmanDEPRECATED, field) lp_field.x, lp_field.y, lp_field.z = llk_field.value or nans lp_field.xStd, lp_field.yStd, lp_field.zStd = llk_field.std or nans lp_field.valid = llk_field.valid for flag in ["inputsOK", "posenetOK", "sensorsOK"]: - setattr(m.livePose, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag)) + setattr(m.deviceMotion, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag)) ops.append((index, as_reader(m))) return ops, [], [] -@migration(inputs=["livePose"]) -def migrate_livePose(msgs): +@migration(inputs=["deviceMotion"]) +def migrate_deviceMotion(msgs): ops = [] - needs_migration = all(msg.livePose.timestamp == 0 for _, msg in msgs if msg.which() == 'livePose') + needs_migration = all(msg.deviceMotion.timestamp == 0 for _, msg in msgs if msg.which() == 'deviceMotion') if not needs_migration: return [], [], [] for index, msg in msgs: - if msg.which() == "livePose": + if msg.which() == "deviceMotion": new_msg = msg.as_builder() - new_msg.livePose.timestamp = msg.logMonoTime + new_msg.deviceMotion.timestamp = msg.logMonoTime ops.append((index, as_reader(new_msg))) return ops, [], [] diff --git a/openpilot/selfdrive/test/process_replay/model_replay.py b/openpilot/selfdrive/test/process_replay/model_replay.py index 3b5038b24c..ba610c6a2b 100755 --- a/openpilot/selfdrive/test/process_replay/model_replay.py +++ b/openpilot/selfdrive/test/process_replay/model_replay.py @@ -152,11 +152,11 @@ def model_replay(lr, frs): dmodeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"cabinCameraState"}, {"cabinEncodeIdx", "carParams", "can"}) if not SEND_EXTRA_INPUTS: - modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] - dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration'] + modeld_logs = [msg for msg in modeld_logs if msg.which() != 'extrinsicsCalibration'] + dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'extrinsicsCalibration'] # initial setup - for s in ('liveCalibration', 'deviceState'): + for s in ('extrinsicsCalibration', 'deviceState'): msg = next(msg for msg in lr if msg.which() == s).as_builder() msg.logMonoTime = lr[0].logMonoTime modeld_logs.insert(1, msg.as_reader()) diff --git a/openpilot/selfdrive/test/process_replay/process_replay.py b/openpilot/selfdrive/test/process_replay/process_replay.py index 346d532140..5abfab2c35 100755 --- a/openpilot/selfdrive/test/process_replay/process_replay.py +++ b/openpilot/selfdrive/test/process_replay/process_replay.py @@ -432,9 +432,9 @@ CONFIGS = [ ProcessConfig( proc_name="selfdrived", pubs=[ - "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", - "longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2", - "cabinCameraState", "narrowRoadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters", + "carState", "deviceState", "pandaStates", "peripheralState", "extrinsicsCalibration", "driverMonitoringState", + "longitudinalPlan", "deviceMotion", "lateralDelay", "vehicleParameters", "radarState", "modelV2", + "cabinCameraState", "narrowRoadCameraState", "wideRoadCameraState", "managerState", "lateralTorqueParameters", "accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance", "alertDebug", ], @@ -448,8 +448,8 @@ CONFIGS = [ ), ProcessConfig( proc_name="controlsd", - pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState", - "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput", + pubs=["vehicleParameters", "lateralTorqueParameters", "modelV2", "selfdriveState", + "extrinsicsCalibration", "deviceMotion", "longitudinalPlan", "carState", "carOutput", "driverMonitoringState", "onroadEvents", "driverAssistance"], subs=["carControl", "controlsState"], ignore=["logMonoTime", ], @@ -460,7 +460,7 @@ CONFIGS = [ ProcessConfig( proc_name="card", pubs=["pandaStates", "carControl", "onroadEvents", "can"], - subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"], + subs=["sendcan", "carState", "carParams", "carOutput", "radarTracks"], ignore=["logMonoTime", "carState.cumLagMs"], init_callback=card_fingerprint_callback, should_recv_callback=card_rcv_callback, @@ -471,7 +471,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="radard", - pubs=["liveTracks", "carState", "modelV2"], + pubs=["radarTracks", "carState", "modelV2"], subs=["radarState"], ignore=["logMonoTime"], init_callback=get_car_params_callback, @@ -479,7 +479,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="plannerd", - pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], + pubs=["modelV2", "carControl", "carState", "controlsState", "vehicleParameters", "radarState", "selfdriveState"], subs=["longitudinalPlan", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, @@ -489,14 +489,14 @@ CONFIGS = [ ProcessConfig( proc_name="calibrationd", pubs=["carState", "cameraOdometry"], - subs=["liveCalibration"], + subs=["extrinsicsCalibration"], ignore=["logMonoTime"], init_callback=get_car_params_callback, should_recv_callback=MessageBasedRcvCallback("cameraOdometry", True), ), ProcessConfig( proc_name="dmonitoringd", - pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"], + pubs=["driverStateV2", "extrinsicsCalibration", "carState", "modelV2", "selfdriveState"], subs=["driverMonitoringState"], ignore=["logMonoTime"], should_recv_callback=MessageBasedRcvCallback("driverStateV2"), @@ -505,9 +505,9 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState" + "cameraOdometry", "accelerometer", "gyroscope", "extrinsicsCalibration", "carState" ], - subs=["livePose"], + subs=["deviceMotion"], ignore=["logMonoTime"], should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), tolerance=NUMPY_TOLERANCE, @@ -515,21 +515,21 @@ CONFIGS = [ ), ProcessConfig( proc_name="paramsd", - pubs=["livePose", "liveCalibration", "carState"], - subs=["liveParameters"], + pubs=["deviceMotion", "extrinsicsCalibration", "carState"], + subs=["vehicleParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose"), + should_recv_callback=MessageBasedRcvCallback("deviceMotion"), tolerance=NUMPY_TOLERANCE, processing_time=0.004, ), ProcessConfig( proc_name="lagd", - pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"], - subs=["liveDelay"], + pubs=["deviceMotion", "extrinsicsCalibration", "carState", "carControl", "controlsState"], + subs=["lateralDelay"], ignore=["logMonoTime"], init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose"), + should_recv_callback=MessageBasedRcvCallback("deviceMotion"), tolerance=NUMPY_TOLERANCE, ), ProcessConfig( @@ -540,16 +540,17 @@ CONFIGS = [ ), ProcessConfig( proc_name="torqued", - pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"], - subs=["liveTorqueParameters"], + pubs=["deviceMotion", "extrinsicsCalibration", "lateralDelay", "carState", "carControl", "carOutput"], + subs=["lateralTorqueParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose", True), + should_recv_callback=MessageBasedRcvCallback("deviceMotion", True), tolerance=NUMPY_TOLERANCE, ), ProcessConfig( proc_name="modeld", - pubs=["deviceState", "narrowRoadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"], + pubs=["deviceState", "narrowRoadCameraState", "wideRoadCameraState", "extrinsicsCalibration", "lateralDelay", + "driverMonitoringState", "carState", "carControl"], subs=["modelV2", "drivingModelData", "cameraOdometry"], ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(), @@ -562,7 +563,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="dmonitoringmodeld", - pubs=["liveCalibration", "cabinCameraState"], + pubs=["extrinsicsCalibration", "cabinCameraState"], subs=["driverStateV2"], ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"], should_recv_callback=MessageBasedRcvCallback("cabinCameraState"), @@ -586,30 +587,30 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> """ Use this to get custom params dict based on provided logs. Useful when replaying following processes: calibrationd, paramsd, torqued - The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. + The params may be based on first or last message of given type (carParams, extrinsicsCalibration, vehicleParameters, lateralTorqueParameters) in the logs. """ car_params = [m for m in lr if m.which() == "carParams"] - live_calibration = [m for m in lr if m.which() == "liveCalibration"] - live_parameters = [m for m in lr if m.which() == "liveParameters"] - live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] + extrinsics_calibration = [m for m in lr if m.which() == "extrinsicsCalibration"] + vehicle_parameters = [m for m in lr if m.which() == "vehicleParameters"] + torque_parameters = [m for m in lr if m.which() == "lateralTorqueParameters"] assert initial_state in ["first", "last"] msg_index = 0 if initial_state == "first" else -1 - assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute" + assert len(car_params) > 0, "carParams required for initial state of vehicleParameters and CarParamsPrevRoute" CP = car_params[msg_index].carParams custom_params = { "CarParamsPrevRoute": CP.as_builder().to_bytes() } - if len(live_calibration) > 0: - custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() - if len(live_parameters) > 0: - custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes() - if len(live_torque_parameters) > 0: - custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() + if len(extrinsics_calibration) > 0: + custom_params["CalibrationParams"] = extrinsics_calibration[msg_index].as_builder().to_bytes() + if len(vehicle_parameters) > 0: + custom_params["LiveParametersV2"] = vehicle_parameters[msg_index].as_builder().to_bytes() + if len(torque_parameters) > 0: + custom_params["LiveTorqueParameters"] = torque_parameters[msg_index].as_builder().to_bytes() return custom_params diff --git a/openpilot/selfdrive/test/test_onroad.py b/openpilot/selfdrive/test/test_onroad.py index fb8cd1393f..6768dc1d98 100755 --- a/openpilot/selfdrive/test/test_onroad.py +++ b/openpilot/selfdrive/test/test_onroad.py @@ -87,8 +87,8 @@ TIMINGS = { "cabinCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], "driverStateV2": [2.5, 0.40], - "livePose": [2.5, 0.35], - "liveParameters": [2.5, 0.35], + "deviceMotion": [2.5, 0.35], + "vehicleParameters": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } diff --git a/openpilot/selfdrive/test/test_power_draw.py b/openpilot/selfdrive/test/test_power_draw.py index 8cad68978f..1d0be24ef0 100755 --- a/openpilot/selfdrive/test/test_power_draw.py +++ b/openpilot/selfdrive/test/test_power_draw.py @@ -95,7 +95,7 @@ class TestPowerDraw(OpenpilotTestCase): return now, msg_counts, time.monotonic() - start_time - SAMPLE_TIME - @mock_messages(['livePose']) + @mock_messages(['deviceMotion']) def test_camera_procs(self, subtests): baseline = get_power() diff --git a/openpilot/selfdrive/ui/layouts/settings/device.py b/openpilot/selfdrive/ui/layouts/settings/device.py index ddc5c23160..093fc738a0 100644 --- a/openpilot/selfdrive/ui/layouts/settings/device.py +++ b/openpilot/selfdrive/ui/layouts/settings/device.py @@ -116,9 +116,9 @@ class DeviceLayout(Widget): calib_bytes = self._params.get("CalibrationParams") if calib_bytes: try: - calib = messaging.log_from_bytes(calib_bytes, log.Event).liveCalibration + calib = messaging.log_from_bytes(calib_bytes, log.Event).extrinsicsCalibration - if calib.calStatus != log.LiveCalibrationData.Status.uncalibrated: + if calib.calStatus != log.ExtrinsicsCalibration.Status.uncalibrated: pitch = math.degrees(calib.rpyCalib[1]) yaw = math.degrees(calib.rpyCalib[2]) desc += tr(" Your device is pointed {:.1f}° {} and {:.1f}° {}.").format(abs(pitch), tr("down") if pitch > 0 else tr("up"), @@ -130,7 +130,7 @@ class DeviceLayout(Widget): lag_bytes = self._params.get("LiveDelay") if lag_bytes: try: - lag_perc = messaging.log_from_bytes(lag_bytes, log.Event).liveDelay.calPerc + lag_perc = messaging.log_from_bytes(lag_bytes, log.Event).lateralDelay.calPerc except Exception: cloudlog.exception("invalid LiveDelay") if lag_perc < 100: @@ -141,7 +141,7 @@ class DeviceLayout(Widget): torque_bytes = self._params.get("LiveTorqueParameters") if torque_bytes: try: - torque = messaging.log_from_bytes(torque_bytes, log.Event).liveTorqueParameters + torque = messaging.log_from_bytes(torque_bytes, log.Event).lateralTorqueParameters # don't add for non-torque cars if torque.useParams: torque_perc = torque.calPerc diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index 46348a9f16..4f4e4b2f6c 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -20,7 +20,7 @@ from openpilot.common.transformations.orientation import rot_from_euler from enum import IntEnum OpState = log.SelfdriveState.OpenpilotState -CALIBRATED = log.LiveCalibrationData.Status.calibrated +CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] @@ -266,11 +266,11 @@ class AugmentedRoadView(CameraView): if not self.device_camera and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))] - # Check if live calibration data is available and valid - if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): + # Check if camera calibration data is available and valid + if not (sm.updated["extrinsicsCalibration"] and sm.valid['extrinsicsCalibration']): return - calib = sm['liveCalibration'] + calib = sm['extrinsicsCalibration'] if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: return @@ -285,7 +285,7 @@ class AugmentedRoadView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: cache_key = ( - ui_state.sm.recv_frame['liveCalibration'], + ui_state.sm.recv_frame['extrinsicsCalibration'], int(self._content_rect.width), int(self._content_rect.height), self.stream_type, diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 4d19850769..dd0d6d9734 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py @@ -100,7 +100,7 @@ class ModelRenderer(Widget): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) # Check if data is up-to-date - if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or + if (sm.recv_frame["extrinsicsCalibration"] < ui_state.started_frame or sm.recv_frame["modelV2"] < ui_state.started_frame): return @@ -112,8 +112,8 @@ class ModelRenderer(Widget): # Update state self._experimental_mode = sm['selfdriveState'].experimentalMode - live_calib = sm['liveCalibration'] - self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0] + extrinsics_calibration = sm['extrinsicsCalibration'] + self._path_offset_z = extrinsics_calibration.height[0] if extrinsics_calibration.height else HEIGHT_INIT[0] if sm.updated['carParams']: self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl diff --git a/openpilot/selfdrive/ui/mici/onroad/torque_bar.py b/openpilot/selfdrive/ui/mici/onroad/torque_bar.py index 6a1d12b6c4..3c0b963666 100644 --- a/openpilot/selfdrive/ui/mici/onroad/torque_bar.py +++ b/openpilot/selfdrive/ui/mici/onroad/torque_bar.py @@ -166,7 +166,7 @@ class TorqueBar(Widget): if ui_state.sm['controlsState'].lateralControlState.which() in ('angleState', 'curvatureState'): controls_state = ui_state.sm['controlsState'] car_state = ui_state.sm['carState'] - live_parameters = ui_state.sm['liveParameters'] + vehicle_parameters = ui_state.sm['vehicleParameters'] car_control = ui_state.sm['carControl'] # Include lateral accel error in estimated torque utilization @@ -176,7 +176,7 @@ class TorqueBar(Widget): # Include road roll in estimated torque utilization # Roll is less accurate near standstill, so reduce its effect at low speed - roll_compensation = live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0]) + roll_compensation = vehicle_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0]) lateral_acceleration = actual_lateral_accel - roll_compensation max_lateral_acceleration = ui_state.CP.maxLateralAccel if ui_state.CP else DEFAULT_MAX_LAT_ACCEL diff --git a/openpilot/selfdrive/ui/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/onroad/augmented_road_view.py index 44d8910a03..cae462e41d 100644 --- a/openpilot/selfdrive/ui/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/onroad/augmented_road_view.py @@ -14,7 +14,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera from openpilot.common.transformations.orientation import rot_from_euler OpState = log.SelfdriveState.OpenpilotState -CALIBRATED = log.LiveCalibrationData.Status.calibrated +CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] @@ -131,11 +131,11 @@ class AugmentedRoadView(CameraView): if not self.device_camera and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))] - # Check if live calibration data is available and valid - if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): + # Check if camera calibration data is available and valid + if not (sm.updated["extrinsicsCalibration"] and sm.valid['extrinsicsCalibration']): return - calib = sm['liveCalibration'] + calib = sm['extrinsicsCalibration'] if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: return @@ -151,7 +151,7 @@ class AugmentedRoadView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: # Check if we can use cached matrix cache_key = ( - ui_state.sm.recv_frame['liveCalibration'], + ui_state.sm.recv_frame['extrinsicsCalibration'], self._content_rect.width, self._content_rect.height, self.stream_type diff --git a/openpilot/selfdrive/ui/onroad/model_renderer.py b/openpilot/selfdrive/ui/onroad/model_renderer.py index 8a40c90025..18fe45d8e1 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/onroad/model_renderer.py @@ -85,7 +85,7 @@ class ModelRenderer(Widget): sm = ui_state.sm # Check if data is up-to-date - if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or + if (sm.recv_frame["extrinsicsCalibration"] < ui_state.started_frame or sm.recv_frame["modelV2"] < ui_state.started_frame): return @@ -97,8 +97,8 @@ class ModelRenderer(Widget): # Update state self._experimental_mode = sm['selfdriveState'].experimentalMode - live_calib = sm['liveCalibration'] - self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0] + extrinsics_calibration = sm['extrinsicsCalibration'] + self._path_offset_z = extrinsics_calibration.height[0] if extrinsics_calibration.height else HEIGHT_INIT[0] if sm.updated['carParams']: self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl diff --git a/openpilot/selfdrive/ui/tests/diff/replay_script.py b/openpilot/selfdrive/ui/tests/diff/replay_script.py index 30e811dbc9..962d409344 100644 --- a/openpilot/selfdrive/ui/tests/diff/replay_script.py +++ b/openpilot/selfdrive/ui/tests/diff/replay_script.py @@ -144,19 +144,19 @@ def set_updater_state(state: str) -> None: def setup_calibration_params() -> None: params = Params() - # live calibration - calib = messaging.new_message('liveCalibration') - calib.liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated - calib.liveCalibration.rpyCalib = [0.0, math.radians(2.5), math.radians(-1.2)] + # camera calibration + calib = messaging.new_message('extrinsicsCalibration') + calib.extrinsicsCalibration.calStatus = log.ExtrinsicsCalibration.Status.calibrated + calib.extrinsicsCalibration.rpyCalib = [0.0, math.radians(2.5), math.radians(-1.2)] params.put("CalibrationParams", calib.to_bytes(), block=True) - # live delay - delay = messaging.new_message('liveDelay') - delay.liveDelay.calPerc = 75 + # lateral delay + delay = messaging.new_message('lateralDelay') + delay.lateralDelay.calPerc = 75 params.put("LiveDelay", delay.to_bytes(), block=True) - # live torque parameters - torque = messaging.new_message('liveTorqueParameters') - torque.liveTorqueParameters.useParams = True - torque.liveTorqueParameters.calPerc = 60 + # lateral torque parameters + torque = messaging.new_message('lateralTorqueParameters') + torque.lateralTorqueParameters.useParams = True + torque.lateralTorqueParameters.calPerc = 60 params.put("LiveTorqueParameters", torque.to_bytes(), block=True) diff --git a/openpilot/selfdrive/ui/ui_state.py b/openpilot/selfdrive/ui/ui_state.py index 943ba46014..42e8086351 100644 --- a/openpilot/selfdrive/ui/ui_state.py +++ b/openpilot/selfdrive/ui/ui_state.py @@ -40,7 +40,7 @@ class UIState: "modelV2", "controlsState", "onroadEvents", - "liveCalibration", + "extrinsicsCalibration", "radarState", "deviceState", "pandaStates", @@ -56,7 +56,7 @@ class UIState: "gpsLocationExternal", "carOutput", "carControl", - "liveParameters", + "vehicleParameters", "testJoystick", "rawAudioData", ] diff --git a/openpilot/tools/jotpluggler/layouts/locationd_debug.json b/openpilot/tools/jotpluggler/layouts/locationd_debug.json index 0541427bc1..53112f31d6 100644 --- a/openpilot/tools/jotpluggler/layouts/locationd_debug.json +++ b/openpilot/tools/jotpluggler/layouts/locationd_debug.json @@ -1 +1 @@ -{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.166588,0.167062,0.166113,0.166588,0.167062,0.166588],"children":[{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/livePose/inputsOK","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":14.542814,"bottom":-5.586039},"curves":[{"name":"/accelerometer/acceleration/v/0","color":"#f14cc1"},{"name":"/accelerometer/acceleration/v/1","color":"#9467bd"},{"name":"/accelerometer/acceleration/v/2","color":"#17becf"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":0.988911,"bottom":-0.745939},"curves":[{"name":"/gyroscope/gyroUncalibrated/v/0","color":"#d62728"},{"name":"/gyroscope/gyroUncalibrated/v/1","color":"#1ac938"},{"name":"/gyroscope/gyroUncalibrated/v/2","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/accelerometer/__valid","color":"#17becf"},{"name":"/gyroscope/__valid","color":"#bcbd22"},{"name":"/carState/__valid","color":"#f14cc1"},{"name":"/liveCalibration/__valid","color":"#1ac938"},{"name":"/cameraOdometry/__valid","color":"#9467bd"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1000000000.292252,"bottom":999999999.735447},"curves":[{"name":"/gyroscope/__logMonoTime","color":"#1f77b4","transform":"derivative"},{"name":"/accelerometer/__logMonoTime","color":"#d62728","transform":"derivative"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":20790107743.93223,"bottom":-529653831.495853},"curves":[{"name":"/accelerometer/timestamp","color":"#bcbd22","transform":"derivative"},{"name":"/gyroscope/timestamp","color":"#1f77b4","transform":"derivative"}]}]}}]} +{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.166588,0.167062,0.166113,0.166588,0.167062,0.166588],"children":[{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/deviceMotion/inputsOK","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":14.542814,"bottom":-5.586039},"curves":[{"name":"/accelerometer/acceleration/v/0","color":"#f14cc1"},{"name":"/accelerometer/acceleration/v/1","color":"#9467bd"},{"name":"/accelerometer/acceleration/v/2","color":"#17becf"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":0.988911,"bottom":-0.745939},"curves":[{"name":"/gyroscope/gyroUncalibrated/v/0","color":"#d62728"},{"name":"/gyroscope/gyroUncalibrated/v/1","color":"#1ac938"},{"name":"/gyroscope/gyroUncalibrated/v/2","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/accelerometer/__valid","color":"#17becf"},{"name":"/gyroscope/__valid","color":"#bcbd22"},{"name":"/carState/__valid","color":"#f14cc1"},{"name":"/extrinsicsCalibration/__valid","color":"#1ac938"},{"name":"/cameraOdometry/__valid","color":"#9467bd"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1000000000.292252,"bottom":999999999.735447},"curves":[{"name":"/gyroscope/__logMonoTime","color":"#1f77b4","transform":"derivative"},{"name":"/accelerometer/__logMonoTime","color":"#d62728","transform":"derivative"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":20790107743.93223,"bottom":-529653831.495853},"curves":[{"name":"/accelerometer/timestamp","color":"#bcbd22","transform":"derivative"},{"name":"/gyroscope/timestamp","color":"#1f77b4","transform":"derivative"}]}]}}]} diff --git a/openpilot/tools/jotpluggler/layouts/max-torque-debug.json b/openpilot/tools/jotpluggler/layouts/max-torque-debug.json index 3a87fb3217..b587b695a9 100644 --- a/openpilot/tools/jotpluggler/layouts/max-torque-debug.json +++ b/openpilot/tools/jotpluggler/layouts/max-torque-debug.json @@ -1 +1 @@ -{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.249724,0.250829,0.249724,0.249724],"children":[{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":6.050533,"bottom":-7.599037},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":5.384416,"bottom":-7.503945},"curves":[{"name":"roll compensated lateral acceleration","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/liveParameters/roll","/carState/steeringPressed","/carControl/latActive"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2, v3, v4):\n if (v3 == 0 and v4 == 1):\n return (value * v1 ** 2) - (v2 * 9.81)\n return 0\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i], v3[__jotpluggler_i], v4[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carState/steeringPressed","color":"#0097ff"},{"name":"/carOutput/actuatorsOutput/torque","color":"#d62728"}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":80.762969,"bottom":-2.181837},"curves":[{"name":"/carState/vEgo","color":"#f14cc1","transform":"scale","scale":2.23694,"offset":0.0}]}]}}]} +{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.249724,0.250829,0.249724,0.249724],"children":[{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":6.050533,"bottom":-7.599037},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":5.384416,"bottom":-7.503945},"curves":[{"name":"roll compensated lateral acceleration","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll","/carState/steeringPressed","/carControl/latActive"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2, v3, v4):\n if (v3 == 0 and v4 == 1):\n return (value * v1 ** 2) - (v2 * 9.81)\n return 0\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i], v3[__jotpluggler_i], v4[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carState/steeringPressed","color":"#0097ff"},{"name":"/carOutput/actuatorsOutput/torque","color":"#d62728"}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":80.762969,"bottom":-2.181837},"curves":[{"name":"/carState/vEgo","color":"#f14cc1","transform":"scale","scale":2.23694,"offset":0.0}]}]}}]} diff --git a/openpilot/tools/jotpluggler/layouts/torque-controller.json b/openpilot/tools/jotpluggler/layouts/torque-controller.json index 7e269e59e6..a794c725d6 100644 --- a/openpilot/tools/jotpluggler/layouts/torque-controller.json +++ b/openpilot/tools/jotpluggler/layouts/torque-controller.json @@ -1 +1 @@ -{"current_tab_index":0,"tabs":[{"name":"Lateral Plan Conformance","root":{"split":"vertical","sizes":[0.250949,0.249051,0.250949,0.249051],"children":[{"title":"desired vs actual lateral acceleration (closer means better conformance to plan)","range":{"left":0.000194,"right":1138.891674,"top":1.858161,"bottom":-1.823407},"curves":[{"name":"/controlsState/lateralControlState/torqueState/actualLateralAccel","color":"#1f77b4"},{"name":"/controlsState/lateralControlState/torqueState/desiredLateralAccel","color":"#d62728"}]},{"title":"desired vs actual lateral acceleration, road-roll factored out (closer means better conformance to plan)","range":{"left":0.000194,"right":1138.891674,"top":2.749816,"bottom":-3.723091},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"controller feed-forward vs actuator output (closer means controller prediction is more accurate)","range":{"left":0.000194,"right":1138.891674,"top":1.978032,"bottom":-1.570956},"curves":[{"name":"/carOutput/actuatorsOutput/torque","color":"#9467bd","transform":"scale","scale":-1.0,"offset":0.0},{"name":"/controlsState/lateralControlState/torqueState/f","color":"#1f77b4"},{"name":"/carState/steeringPressed","color":"#ff000f"}]},{"title":"vehicle speed","range":{"left":0.000194,"right":1138.891674,"top":105.981304,"bottom":-2.709314},"curves":[{"name":"carState.vEgo mph","color":"#d62728","custom_python":{"linked_source":"/carState/vEgo","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value * 2.23694\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"carState.vEgo kmh","color":"#1ac938","custom_python":{"linked_source":"/carState/vEgo","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value * 3.6\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"/carState/vEgo","color":"#ff7f0e"}]}]}},{"name":"Vehicle Dynamics","root":{"split":"vertical","sizes":[0.334282,0.331437,0.334282],"children":[{"title":"configured-initial vs online-learned steerRatio, set configured value to match learned","range":{"left":0.0,"right":1138.816328,"top":19.665784,"bottom":19.359553},"curves":[{"name":"/carParams/steerRatio","color":"#1f77b4"},{"name":"/liveParameters/steerRatio","color":"#1ac938"}]},{"title":"configured-initial vs online-learned tireStiffnessRatio, set configured value to match learned","range":{"left":0.0,"right":1138.816328,"top":1.11221,"bottom":0.995631},"curves":[{"name":"/carParams/tireStiffnessFactor","color":"#d62728"},{"name":"/liveParameters/stiffnessFactor","color":"#ff7f0e"}]},{"title":"live steering angle offsets for straight-ahead driving, large values here may indicate alignment problems","range":{"left":0.0,"right":1138.816328,"top":-1.081041,"bottom":-4.494133},"curves":[{"name":"/liveParameters/angleOffsetAverageDeg","color":"#f14cc1"},{"name":"/liveParameters/angleOffsetDeg","color":"#9467bd"}]}]}},{"name":"Actuator Performance","root":{"split":"vertical","sizes":[0.333333,0.333333,0.333333],"children":[{"title":"offline-calculated vs online-learned lateral accel scaling factor, accel obtained from 100% actuator output","range":{"left":0.0,"right":1138.920072,"top":1.21611,"bottom":0.539474},"curves":[{"name":"/liveTorqueParameters/latAccelFactorFiltered","color":"#1f77b4"},{"name":"/liveTorqueParameters/latAccelFactorRaw","color":"#d62728"},{"name":"/carParams/lateralTuning/torque/latAccelFactor","color":"#1c9222"}]},{"title":"learned lateral accel offset, vehicle-specific compensation to obtain true zero lateral accel","range":{"left":0.0,"right":1138.920072,"top":-0.304367,"bottom":-0.418688},"curves":[{"name":"/liveTorqueParameters/latAccelOffsetFiltered","color":"#1ac938"},{"name":"/liveTorqueParameters/latAccelOffsetRaw","color":"#ff7f0e"}]},{"title":"offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel","range":{"left":0.0,"right":1138.920072,"top":0.226389,"bottom":0.15805},"curves":[{"name":"/liveTorqueParameters/frictionCoefficientFiltered","color":"#f14cc1"},{"name":"/liveTorqueParameters/frictionCoefficientRaw","color":"#9467bd"},{"name":"/carParams/lateralTuning/torque/friction","color":"#1c9222"}]}]}},{"name":"Actuator Delay","root":{"split":"vertical","sizes":[0.30441,0.358464,0.337127],"children":[{"title":"actuator lag learning state, 0 = learning, 1 = learned/applying, 2 = invalid","range":{"left":0.0,"right":1138.749979,"top":1.025,"bottom":-0.025},"curves":[{"name":"/liveDelay/status","color":"#ff7f0e"}]},{"title":"offline default vs online estimated steering actuator lag","range":{"left":0.0,"right":1138.749979,"top":0.419648,"bottom":0.318362},"curves":[{"name":"/liveDelay/lateralDelay","color":"#1f77b4"},{"name":"/liveDelay/lateralDelayEstimate","color":"#d62728"},{"name":"opendbc default steering lag","color":"#1ac938","custom_python":{"linked_source":"/carParams/steerActuatorDelay","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value + 0.2\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"online estimated steering actuator lag, standard deviation","range":{"left":0.0,"right":1138.749979,"top":0.06732,"bottom":-0.001642},"curves":[{"name":"/liveDelay/lateralDelayEstimateStd","color":"#f14cc1"}]}]}},{"name":"Controls Performance","root":{"split":"vertical","sizes":[0.265655,0.251898,0.245731,0.236717],"children":[{"title":"rate-of-change limits on steering actuator (blue = original, green = rate-limited before CAN output)","range":{"left":0.000194,"right":1138.891921,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carControl/actuators/torque","color":"#0c00f2"},{"name":"/carOutput/actuatorsOutput/torque","color":"#2cd63a"}]},{"title":"controller feed-forward vs actuator output (closer means controller prediction is more accurate)","range":{"left":0.000194,"right":1138.891921,"top":1.978032,"bottom":-1.570956},"curves":[{"name":"/carOutput/actuatorsOutput/torque","color":"#9467bd","transform":"scale","scale":-1.0,"offset":0.0},{"name":"/controlsState/lateralControlState/torqueState/f","color":"#1f77b4"},{"name":"/carState/steeringPressed","color":"#ff000f"}]},{"title":"proportional, integral, and feed-forward terms (actuator output = sum of PIF terms)","range":{"left":0.000194,"right":1138.891921,"top":2.099784,"bottom":-4.027542},"curves":[{"name":"/controlsState/lateralControlState/torqueState/f","color":"#0ab027"},{"name":"/controlsState/lateralControlState/torqueState/p","color":"#d62728"},{"name":"/controlsState/lateralControlState/torqueState/i","color":"#ffaf00"},{"name":"Zero","color":"#756a6a","custom_python":{"linked_source":"/carState/canValid","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return (0)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"road roll angle, from openpilot localizer","range":{"left":0.000194,"right":1138.891921,"top":0.109446,"bottom":-0.045525},"curves":[{"name":"/liveParameters/roll","color":"#f14cc1"}]}]}}]} +{"current_tab_index":0,"tabs":[{"name":"Lateral Plan Conformance","root":{"split":"vertical","sizes":[0.250949,0.249051,0.250949,0.249051],"children":[{"title":"desired vs actual lateral acceleration (closer means better conformance to plan)","range":{"left":0.000194,"right":1138.891674,"top":1.858161,"bottom":-1.823407},"curves":[{"name":"/controlsState/lateralControlState/torqueState/actualLateralAccel","color":"#1f77b4"},{"name":"/controlsState/lateralControlState/torqueState/desiredLateralAccel","color":"#d62728"}]},{"title":"desired vs actual lateral acceleration, road-roll factored out (closer means better conformance to plan)","range":{"left":0.000194,"right":1138.891674,"top":2.749816,"bottom":-3.723091},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"controller feed-forward vs actuator output (closer means controller prediction is more accurate)","range":{"left":0.000194,"right":1138.891674,"top":1.978032,"bottom":-1.570956},"curves":[{"name":"/carOutput/actuatorsOutput/torque","color":"#9467bd","transform":"scale","scale":-1.0,"offset":0.0},{"name":"/controlsState/lateralControlState/torqueState/f","color":"#1f77b4"},{"name":"/carState/steeringPressed","color":"#ff000f"}]},{"title":"vehicle speed","range":{"left":0.000194,"right":1138.891674,"top":105.981304,"bottom":-2.709314},"curves":[{"name":"carState.vEgo mph","color":"#d62728","custom_python":{"linked_source":"/carState/vEgo","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value * 2.23694\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"carState.vEgo kmh","color":"#1ac938","custom_python":{"linked_source":"/carState/vEgo","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value * 3.6\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"/carState/vEgo","color":"#ff7f0e"}]}]}},{"name":"Vehicle Dynamics","root":{"split":"vertical","sizes":[0.334282,0.331437,0.334282],"children":[{"title":"configured-initial vs online-learned steerRatio, set configured value to match learned","range":{"left":0.0,"right":1138.816328,"top":19.665784,"bottom":19.359553},"curves":[{"name":"/carParams/steerRatio","color":"#1f77b4"},{"name":"/vehicleParameters/steerRatio","color":"#1ac938"}]},{"title":"configured-initial vs online-learned tireStiffnessRatio, set configured value to match learned","range":{"left":0.0,"right":1138.816328,"top":1.11221,"bottom":0.995631},"curves":[{"name":"/carParams/tireStiffnessFactor","color":"#d62728"},{"name":"/vehicleParameters/stiffnessFactor","color":"#ff7f0e"}]},{"title":"online-learned steering angle offsets for straight-ahead driving, large values here may indicate alignment problems","range":{"left":0.0,"right":1138.816328,"top":-1.081041,"bottom":-4.494133},"curves":[{"name":"/vehicleParameters/angleOffsetAverageDeg","color":"#f14cc1"},{"name":"/vehicleParameters/angleOffsetDeg","color":"#9467bd"}]}]}},{"name":"Actuator Performance","root":{"split":"vertical","sizes":[0.333333,0.333333,0.333333],"children":[{"title":"offline-calculated vs online-learned lateral accel scaling factor, accel obtained from 100% actuator output","range":{"left":0.0,"right":1138.920072,"top":1.21611,"bottom":0.539474},"curves":[{"name":"/lateralTorqueParameters/latAccelFactorFiltered","color":"#1f77b4"},{"name":"/lateralTorqueParameters/latAccelFactorRaw","color":"#d62728"},{"name":"/carParams/lateralTuning/torque/latAccelFactor","color":"#1c9222"}]},{"title":"learned lateral accel offset, vehicle-specific compensation to obtain true zero lateral accel","range":{"left":0.0,"right":1138.920072,"top":-0.304367,"bottom":-0.418688},"curves":[{"name":"/lateralTorqueParameters/latAccelOffsetFiltered","color":"#1ac938"},{"name":"/lateralTorqueParameters/latAccelOffsetRaw","color":"#ff7f0e"}]},{"title":"offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel","range":{"left":0.0,"right":1138.920072,"top":0.226389,"bottom":0.15805},"curves":[{"name":"/lateralTorqueParameters/frictionCoefficientFiltered","color":"#f14cc1"},{"name":"/lateralTorqueParameters/frictionCoefficientRaw","color":"#9467bd"},{"name":"/carParams/lateralTuning/torque/friction","color":"#1c9222"}]}]}},{"name":"Actuator Delay","root":{"split":"vertical","sizes":[0.30441,0.358464,0.337127],"children":[{"title":"actuator lag learning state, 0 = learning, 1 = learned/applying, 2 = invalid","range":{"left":0.0,"right":1138.749979,"top":1.025,"bottom":-0.025},"curves":[{"name":"/lateralDelay/status","color":"#ff7f0e"}]},{"title":"offline default vs online estimated steering actuator lag","range":{"left":0.0,"right":1138.749979,"top":0.419648,"bottom":0.318362},"curves":[{"name":"/lateralDelay/lateralDelay","color":"#1f77b4"},{"name":"/lateralDelay/lateralDelayEstimate","color":"#d62728"},{"name":"opendbc default steering lag","color":"#1ac938","custom_python":{"linked_source":"/carParams/steerActuatorDelay","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return value + 0.2\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"online estimated steering actuator lag, standard deviation","range":{"left":0.0,"right":1138.749979,"top":0.06732,"bottom":-0.001642},"curves":[{"name":"/lateralDelay/lateralDelayEstimateStd","color":"#f14cc1"}]}]}},{"name":"Controls Performance","root":{"split":"vertical","sizes":[0.265655,0.251898,0.245731,0.236717],"children":[{"title":"rate-of-change limits on steering actuator (blue = original, green = rate-limited before CAN output)","range":{"left":0.000194,"right":1138.891921,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carControl/actuators/torque","color":"#0c00f2"},{"name":"/carOutput/actuatorsOutput/torque","color":"#2cd63a"}]},{"title":"controller feed-forward vs actuator output (closer means controller prediction is more accurate)","range":{"left":0.000194,"right":1138.891921,"top":1.978032,"bottom":-1.570956},"curves":[{"name":"/carOutput/actuatorsOutput/torque","color":"#9467bd","transform":"scale","scale":-1.0,"offset":0.0},{"name":"/controlsState/lateralControlState/torqueState/f","color":"#1f77b4"},{"name":"/carState/steeringPressed","color":"#ff000f"}]},{"title":"proportional, integral, and feed-forward terms (actuator output = sum of PIF terms)","range":{"left":0.000194,"right":1138.891921,"top":2.099784,"bottom":-4.027542},"curves":[{"name":"/controlsState/lateralControlState/torqueState/f","color":"#0ab027"},{"name":"/controlsState/lateralControlState/torqueState/p","color":"#d62728"},{"name":"/controlsState/lateralControlState/torqueState/i","color":"#ffaf00"},{"name":"Zero","color":"#756a6a","custom_python":{"linked_source":"/carState/canValid","additional_sources":[],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value):\n return (0)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"road roll angle, from openpilot localizer","range":{"left":0.000194,"right":1138.891921,"top":0.109446,"bottom":-0.045525},"curves":[{"name":"/vehicleParameters/roll","color":"#f14cc1"}]}]}}]} diff --git a/openpilot/tools/joystick/joystickd.py b/openpilot/tools/joystick/joystickd.py index 2b84683863..f8a2598361 100755 --- a/openpilot/tools/joystick/joystickd.py +++ b/openpilot/tools/joystick/joystickd.py @@ -21,7 +21,7 @@ def joystickd_thread(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) VM = VehicleModel(CP) - sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL) + sm = messaging.SubMaster(['carState', 'onroadEvents', 'vehicleParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL) pm = messaging.PubMaster(['carControl', 'controlsState']) rk = Ratekeeper(100, print_delay_threshold=None) @@ -55,7 +55,7 @@ def joystickd_thread(): if CC.latActive: max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5) - max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll)) + max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['vehicleParameters'].roll)) actuators.torque = float(np.clip(joystick_axes[1], -1, 1)) actuators.steeringAngleDeg, actuators.curvature = actuators.torque * max_angle, actuators.torque * -max_curvature @@ -67,7 +67,7 @@ def joystickd_thread(): controlsState = cs_msg.controlsState controlsState.lateralControlState.init('debugState') - lp = sm['liveParameters'] + lp = sm['vehicleParameters'] steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg) controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll) diff --git a/openpilot/tools/longitudinal_maneuvers/generate_report.py b/openpilot/tools/longitudinal_maneuvers/generate_report.py index dbd9f6db91..2d7f81a7f8 100755 --- a/openpilot/tools/longitudinal_maneuvers/generate_report.py +++ b/openpilot/tools/longitudinal_maneuvers/generate_report.py @@ -44,14 +44,14 @@ def report(platform, route, _description, CP, ID, maneuvers): t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True) t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True) t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True) - t_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], strict=True) + t_deviceMotion, deviceMotion = zip(*[(m.logMonoTime, m.deviceMotion) for m in msgs if m.which() == 'deviceMotion'], strict=True) t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True) # make time relative seconds t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl] t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput] t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState] - t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose] + t_deviceMotion = [(t - t_deviceMotion[0]) / 1e9 for t in t_deviceMotion] t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan] # maneuver validity @@ -70,7 +70,7 @@ def report(platform, route, _description, CP, ID, maneuvers): # Localizer is noisy, require two consecutive 20Hz frames above threshold prev_crossed = False - for t, lp in zip(t_livePose, livePose, strict=True): + for t, lp in zip(t_deviceMotion, deviceMotion, strict=True): crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x) if crossed and prev_crossed: builder.append(f', crossed in {t:.3f}s') @@ -95,7 +95,7 @@ def report(platform, route, _description, CP, ID, maneuvers): ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6) ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6) ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6) - ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6) + ax[0].plot(t_deviceMotion, [m.accelerationDevice.x for m in deviceMotion], label='deviceMotion.accelerationDevice.x', linewidth=6) # TODO localizer accel ax[0].set_ylabel('Acceleration (m/s^2)') #ax[0].set_ylim(-6.5, 6.5) diff --git a/openpilot/tools/plotjuggler/layouts/locationd_debug.xml b/openpilot/tools/plotjuggler/layouts/locationd_debug.xml index 5377e1535c..6e1cd35039 100644 --- a/openpilot/tools/plotjuggler/layouts/locationd_debug.xml +++ b/openpilot/tools/plotjuggler/layouts/locationd_debug.xml @@ -8,7 +8,7 @@ - + @@ -36,7 +36,7 @@ - + @@ -97,4 +97,3 @@ - diff --git a/openpilot/tools/plotjuggler/layouts/max-torque-debug.xml b/openpilot/tools/plotjuggler/layouts/max-torque-debug.xml index 9a6693165e..2089b826d9 100644 --- a/openpilot/tools/plotjuggler/layouts/max-torque-debug.xml +++ b/openpilot/tools/plotjuggler/layouts/max-torque-debug.xml @@ -62,7 +62,7 @@ return 0 /controlsState/curvature /carState/vEgo - /liveParameters/roll + /vehicleParameters/roll /carState/steeringPressed /carControl/latActive @@ -73,7 +73,7 @@ return 0 /controlsState/desiredCurvature /carState/vEgo - /liveParameters/roll + /vehicleParameters/roll @@ -82,11 +82,10 @@ return 0 /controlsState/curvature /carState/vEgo - /liveParameters/roll + /vehicleParameters/roll - diff --git a/openpilot/tools/plotjuggler/layouts/torque-controller.xml b/openpilot/tools/plotjuggler/layouts/torque-controller.xml index 8e9a1a8526..671b47c355 100644 --- a/openpilot/tools/plotjuggler/layouts/torque-controller.xml +++ b/openpilot/tools/plotjuggler/layouts/torque-controller.xml @@ -53,7 +53,7 @@ - + @@ -61,15 +61,15 @@ - + - + - - + + @@ -82,8 +82,8 @@ - - + + @@ -91,16 +91,16 @@ - - + + - - + + @@ -114,15 +114,15 @@ - + - - + + @@ -130,7 +130,7 @@ - + @@ -174,7 +174,7 @@ - + @@ -221,7 +221,7 @@ /controlsState/desiredCurvature /carState/vEgo - /liveParameters/roll + /vehicleParameters/roll @@ -230,7 +230,7 @@ /controlsState/curvature /carState/vEgo - /liveParameters/roll + /vehicleParameters/roll @@ -247,4 +247,3 @@ - diff --git a/openpilot/tools/replay/consoleui.cc b/openpilot/tools/replay/consoleui.cc index 2d21b4efc0..aa58cc959d 100644 --- a/openpilot/tools/replay/consoleui.cc +++ b/openpilot/tools/replay/consoleui.cc @@ -60,7 +60,7 @@ ExitHandler do_exit; } // namespace -ConsoleUI::ConsoleUI(Replay *replay) : replay(replay), sm({"carState", "liveParameters"}) { +ConsoleUI::ConsoleUI(Replay *replay) : replay(replay), sm({"carState", "vehicleParameters"}) { // Initialize curses initscr(); clear(); @@ -174,7 +174,7 @@ void ConsoleUI::updateStatus() { std::string current_segment = " - " + std::to_string((int)(replay->currentSeconds() / 60)); write_item(0, 25, "TIME: ", time_string, current_segment, true); - auto p = sm["liveParameters"].getLiveParameters(); + auto p = sm["vehicleParameters"].getVehicleParameters(); write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); write_item(1, 25, "SPEED: ", util::string_format("%.2f", sm["carState"].getCarState().getVEgo()), " m/s"); write_item(2, 0, "STEER RATIO: ", util::string_format("%.2f", p.getSteerRatio()), ""); diff --git a/openpilot/tools/replay/ui.py b/openpilot/tools/replay/ui.py index 16c050f8a6..7f34f1303d 100755 --- a/openpilot/tools/replay/ui.py +++ b/openpilot/tools/replay/ui.py @@ -76,12 +76,12 @@ def ui_thread(addr): 'longitudinalPlan', 'carControl', 'radarState', - 'liveCalibration', + 'extrinsicsCalibration', 'controlsState', 'selfdriveState', - 'liveTracks', + 'radarTracks', 'modelV2', - 'liveParameters', + 'vehicleParameters', 'narrowRoadCameraState', ], addr=addr, @@ -195,10 +195,10 @@ def ui_thread(addr): plot_lead(sm['radarState'], top_down) # draw all radar points - maybe_update_radar_points(sm['liveTracks'].points, top_down[1]) + maybe_update_radar_points(sm['radarTracks'].points, top_down[1]) - if sm.updated['liveCalibration'] and num_px: - rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) + if sm.updated['extrinsicsCalibration'] and num_px: + rpyCalib = np.asarray(sm['extrinsicsCalibration'].rpyCalib) calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale) # Update overlay texture (RGB img -> RGBA with non-black pixels visible) @@ -232,10 +232,10 @@ def ui_thread(addr): ("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), YELLOW), ("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), YELLOW), None, - ("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", YELLOW), - ("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", YELLOW), - ("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", YELLOW), - ("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), YELLOW), + ("ANGLE OFFSET (AVG): " + str(round(sm['vehicleParameters'].angleOffsetAverageDeg, 2)) + " deg", YELLOW), + ("ANGLE OFFSET (INSTANT): " + str(round(sm['vehicleParameters'].angleOffsetDeg, 2)) + " deg", YELLOW), + ("STIFFNESS: " + str(round(sm['vehicleParameters'].stiffnessFactor * 100.0, 2)) + " %", YELLOW), + ("STEER RATIO: " + str(round(sm['vehicleParameters'].steerRatio, 2)), YELLOW), ] for i, line in enumerate(lines): diff --git a/tools/scripts/car/max_lat_accel.py b/tools/scripts/car/max_lat_accel.py index dc44e8ac40..b76b3302cc 100755 --- a/tools/scripts/car/max_lat_accel.py +++ b/tools/scripts/car/max_lat_accel.py @@ -62,8 +62,8 @@ def find_events(lr: LogReader, extrapolate: bool = False, qlog: bool = False) -> elif msg.which() == 'controlsState': curvature = msg.controlsState.curvature - elif msg.which() == 'liveParameters': - roll = msg.liveParameters.roll + elif msg.which() == 'vehicleParameters': + roll = msg.vehicleParameters.roll if lat_active > min_lat_active and steering_unpressed > min_steering_unpressed and requesting_max > min_requesting_max: # TODO: record max lat accel at the end of the event, need to use the past lat accel as overriding can happen before we detect it diff --git a/tools/scripts/cycle_alerts.py b/tools/scripts/cycle_alerts.py index cf4a1d8999..f78dfe1090 100755 --- a/tools/scripts/cycle_alerts.py +++ b/tools/scripts/cycle_alerts.py @@ -54,8 +54,8 @@ def cycle_alerts(duration=200, is_metric=False): CS = car.CarState.new_message() CP = CarInterface.get_non_essential_params("HONDA_CIVIC") - sm = messaging.SubMaster(['deviceState', 'pandaStates', 'narrowRoadCameraState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'livePose', + sm = messaging.SubMaster(['deviceState', 'pandaStates', 'narrowRoadCameraState', 'modelV2', 'extrinsicsCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'deviceMotion', 'managerState'] + cameras) pm = messaging.PubMaster(['selfdriveState', 'pandaStates', 'deviceState']) @@ -87,7 +87,7 @@ def cycle_alerts(duration=200, is_metric=False): procs[i].shouldBeRunning = True sm['managerState'].processes = procs - sm['liveCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))] + sm['extrinsicsCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))] for s in sm.data.keys(): prob = 0.3 if s in cameras else 0.08