diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index e0784eb8e6cbf4..edcf42ebcda9a8 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -8,6 +8,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog +from openpilot.common.transformations.orientation import rot_from_euler from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator @@ -77,6 +78,8 @@ def __init__(self, CP, decimated=False, track_all_points=False): self.offline_friction = CP.lateralTuning.torque.friction self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor + self.calib_from_device = np.eye(3) + self.reset() initial_params = { @@ -171,12 +174,18 @@ def handle_log(self, t, which, msg): # 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": + device_from_calib = rot_from_euler(np.array(msg.rpyCalib)) + self.calib_from_device = device_from_calib.T # calculate lateral accel from past steering torque - elif which == "liveLocationKalman": + elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: - yaw_rate = msg.angularVelocityCalibrated.value[2] - roll = msg.orientationNED.value[0] + angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z]) + angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device) + + yaw_rate = angular_velocity_calibrated[2] + roll = msg.orientationNED.x # 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) @@ -233,7 +242,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose') params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) @@ -246,7 +255,7 @@ def main(demo=False): t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) - # 4Hz driven by liveLocationKalman + # 4Hz driven by livePose if sm.frame % 5 == 0: pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 84669a8bff7f11..1731bf2e1e28d8 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -390,7 +390,7 @@ def calibration_rcv_callback(msg, cfg, frame): def torqued_rcv_callback(msg, cfg, frame): # should_recv always true to increment frame - return (frame - 1) == 0 or msg.which() == 'liveLocationKalman' + return (frame - 1) == 0 or msg.which() == 'livePose' def dmonitoringmodeld_rcv_callback(msg, cfg, frame): @@ -555,7 +555,7 @@ def locationd_config_pubsub_callback(params, cfg, lr): ), ProcessConfig( proc_name="torqued", - pubs=["liveLocationKalman", "carState", "carControl", "carOutput"], + pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"], subs=["liveTorqueParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4da168c2703fb6..3af1f282fbeaa9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3e8feca20cd64f6a05dcbbdc0ace04a8c12bf86d \ No newline at end of file +7eb60abe8030b97f79f858315d67d080704733f7 \ No newline at end of file