Rename "live" cereal services (#38601)

This commit is contained in:
Adeeb Shihadeh
2026-08-10 09:30:22 -07:00
committed by GitHub
parent 434fbad5fc
commit 6d5d1f6917
64 changed files with 460 additions and 460 deletions
+12 -12
View File
@@ -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;
@@ -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),
}
+6 -6
View File
@@ -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),
+2 -2
View File
@@ -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
}
+9 -9
View File
@@ -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
+1
View File
@@ -77,6 +77,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},
+4 -4
View File
@@ -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"""
+12 -12
View File
@@ -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,
@@ -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
@@ -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')
@@ -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
+1 -1
View File
@@ -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:
+2 -2
View File
@@ -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)
@@ -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):
@@ -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)
@@ -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)
@@ -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)
+28 -28
View File
@@ -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()
+9 -9
View File
@@ -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
+29 -29
View File
@@ -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)
+19 -19
View File
@@ -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__":
+37 -37
View File
@@ -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__":
@@ -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)
+25 -25
View File
@@ -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):
@@ -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():
@@ -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)
@@ -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
+32 -32
View File
@@ -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:
@@ -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)
+2 -2
View File
@@ -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
+8 -8
View File
@@ -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)
@@ -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
+1 -1
View File
@@ -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,
+9 -9
View File
@@ -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:
+4 -4
View File
@@ -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:
+17 -16
View File
@@ -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
+3 -3
View File
@@ -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):
@@ -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
@@ -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
@@ -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, [], []
@@ -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())
@@ -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
+2 -2
View File
@@ -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],
}
+1 -1
View File
@@ -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()
@@ -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
@@ -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,
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)
+2 -2
View File
@@ -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",
]
@@ -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"}]}]}}]}
@@ -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}]}]}}]}
File diff suppressed because one or more lines are too long
+3 -3
View File
@@ -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)
@@ -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', <strong>crossed in {t:.3f}s</strong>')
@@ -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)
@@ -8,7 +8,7 @@
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
<range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
<limitY/>
<curve color="#ff7f0e" name="/livePose/inputsOK"/>
<curve color="#ff7f0e" name="/deviceMotion/inputsOK"/>
</plot>
</DockArea>
<DockArea name="...">
@@ -36,7 +36,7 @@
<curve color="#17becf" name="/accelerometer/__valid"/>
<curve color="#bcbd22" name="/gyroscope/__valid"/>
<curve color="#f14cc1" name="/carState/__valid"/>
<curve color="#1ac938" name="/liveCalibration/__valid"/>
<curve color="#1ac938" name="/extrinsicsCalibration/__valid"/>
<curve color="#9467bd" name="/cameraOdometry/__valid"/>
</plot>
</DockArea>
@@ -97,4 +97,3 @@
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
</root>
@@ -62,7 +62,7 @@ return 0</function>
<linked_source>/controlsState/curvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
<v2>/vehicleParameters/roll</v2>
<v3>/carState/steeringPressed</v3>
<v4>/carControl/latActive</v4>
</additional_sources>
@@ -73,7 +73,7 @@ return 0</function>
<linked_source>/controlsState/desiredCurvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
<v2>/vehicleParameters/roll</v2>
</additional_sources>
</snippet>
<snippet name="Actual lateral accel (roll compensated)">
@@ -82,11 +82,10 @@ return 0</function>
<linked_source>/controlsState/curvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
<v2>/vehicleParameters/roll</v2>
</additional_sources>
</snippet>
</customMathEquations>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
@@ -53,7 +53,7 @@
<range top="19.665784" bottom="19.359553" right="1138.816328" left="0.000000"/>
<limitY/>
<curve name="/carParams/steerRatio" color="#1f77b4"/>
<curve name="/liveParameters/steerRatio" color="#1ac938"/>
<curve name="/vehicleParameters/steerRatio" color="#1ac938"/>
</plot>
</DockArea>
<DockArea name="configured-initial vs online-learned tireStiffnessRatio, set configured value to match learned">
@@ -61,15 +61,15 @@
<range top="1.112210" bottom="0.995631" right="1138.816328" left="0.000000"/>
<limitY/>
<curve name="/carParams/tireStiffnessFactor" color="#d62728"/>
<curve name="/liveParameters/stiffnessFactor" color="#ff7f0e"/>
<curve name="/vehicleParameters/stiffnessFactor" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="live steering angle offsets for straight-ahead driving, large values here may indicate alignment problems">
<DockArea name="online-learned steering angle offsets for straight-ahead driving, large values here may indicate alignment problems">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="-1.081041" bottom="-4.494133" right="1138.816328" left="0.000000"/>
<limitY/>
<curve name="/liveParameters/angleOffsetAverageDeg" color="#f14cc1"/>
<curve name="/liveParameters/angleOffsetDeg" color="#9467bd"/>
<curve name="/vehicleParameters/angleOffsetAverageDeg" color="#f14cc1"/>
<curve name="/vehicleParameters/angleOffsetDeg" color="#9467bd"/>
</plot>
</DockArea>
</DockSplitter>
@@ -82,8 +82,8 @@
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.216110" bottom="0.539474" right="1138.920072" left="0.000000"/>
<limitY/>
<curve name="/liveTorqueParameters/latAccelFactorFiltered" color="#1f77b4"/>
<curve name="/liveTorqueParameters/latAccelFactorRaw" color="#d62728"/>
<curve name="/lateralTorqueParameters/latAccelFactorFiltered" color="#1f77b4"/>
<curve name="/lateralTorqueParameters/latAccelFactorRaw" color="#d62728"/>
<curve name="/carParams/lateralTuning/torque/latAccelFactor" color="#1c9222"/>
</plot>
</DockArea>
@@ -91,16 +91,16 @@
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="-0.304367" bottom="-0.418688" right="1138.920072" left="0.000000"/>
<limitY/>
<curve name="/liveTorqueParameters/latAccelOffsetFiltered" color="#1ac938"/>
<curve name="/liveTorqueParameters/latAccelOffsetRaw" color="#ff7f0e"/>
<curve name="/lateralTorqueParameters/latAccelOffsetFiltered" color="#1ac938"/>
<curve name="/lateralTorqueParameters/latAccelOffsetRaw" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.226389" bottom="0.158050" right="1138.920072" left="0.000000"/>
<limitY/>
<curve name="/liveTorqueParameters/frictionCoefficientFiltered" color="#f14cc1"/>
<curve name="/liveTorqueParameters/frictionCoefficientRaw" color="#9467bd"/>
<curve name="/lateralTorqueParameters/frictionCoefficientFiltered" color="#f14cc1"/>
<curve name="/lateralTorqueParameters/frictionCoefficientRaw" color="#9467bd"/>
<curve name="/carParams/lateralTuning/torque/friction" color="#1c9222"/>
</plot>
</DockArea>
@@ -114,15 +114,15 @@
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.025000" bottom="-0.025000" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/status" color="#ff7f0e"/>
<curve name="/lateralDelay/status" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="offline default vs online estimated steering actuator lag">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.419648" bottom="0.318362" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/lateralDelay" color="#1f77b4"/>
<curve name="/liveDelay/lateralDelayEstimate" color="#d62728"/>
<curve name="/lateralDelay/lateralDelay" color="#1f77b4"/>
<curve name="/lateralDelay/lateralDelayEstimate" color="#d62728"/>
<curve name="opendbc default steering lag" color="#1ac938"/>
</plot>
</DockArea>
@@ -130,7 +130,7 @@
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.067320" bottom="-0.001642" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/lateralDelayEstimateStd" color="#f14cc1"/>
<curve name="/lateralDelay/lateralDelayEstimateStd" color="#f14cc1"/>
</plot>
</DockArea>
</DockSplitter>
@@ -174,7 +174,7 @@
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.109446" bottom="-0.045525" right="1138.891921" left="0.000194"/>
<limitY/>
<curve name="/liveParameters/roll" color="#f14cc1"/>
<curve name="/vehicleParameters/roll" color="#f14cc1"/>
</plot>
</DockArea>
</DockSplitter>
@@ -221,7 +221,7 @@
<linked_source>/controlsState/desiredCurvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
<v2>/vehicleParameters/roll</v2>
</additional_sources>
</snippet>
<snippet name="Actual lateral accel (roll compensated)">
@@ -230,7 +230,7 @@
<linked_source>/controlsState/curvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
<v2>/vehicleParameters/roll</v2>
</additional_sources>
</snippet>
<snippet name="opendbc default steering lag">
@@ -247,4 +247,3 @@
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
+2 -2
View File
@@ -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()), "");
+10 -10
View File
@@ -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):
+2 -2
View File
@@ -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
+3 -3
View File
@@ -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