From 80bc61dc6c36dcac018203dfbcdd16b4a993c9cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Tue, 13 Aug 2024 21:11:16 -0700 Subject: [PATCH] controlsd: use livePose (#33283) * Pose calibrator * Fix static analysis * Fix static * Fix test_latcontrol * Fix test_latcontrol * Update services in process replay * Fix static * Matmul not mul * Add assertion * Move pose calibration to data_sample * Update ref commit * Remove llk from cycle alerts * Deprecated nogps event * Switch power_draw to lp * Bring back noGps alert * Add handling code back * get_bool * Bring inputsok back old-commit-hash: 9734015bbb6d448bb6b0fb453370ec702fa73106 --- common/mock/__init__.py | 4 +- common/mock/generators.py | 26 +++---- selfdrive/controls/controlsd.py | 45 ++++++++----- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 7 +- .../controls/lib/tests/test_latcontrol.py | 9 ++- selfdrive/debug/cycle_alerts.py | 2 +- selfdrive/locationd/helpers.py | 67 +++++++++++++++++++ selfdrive/locationd/paramsd.py | 25 +++---- selfdrive/locationd/torqued.py | 17 +++-- .../test/process_replay/process_replay.py | 10 ++- selfdrive/test/process_replay/ref_commit | 2 +- system/hardware/tici/tests/test_power_draw.py | 2 +- 15 files changed, 148 insertions(+), 74 deletions(-) diff --git a/common/mock/__init__.py b/common/mock/__init__.py index 8c86bbd394..4b01dfe841 100644 --- a/common/mock/__init__.py +++ b/common/mock/__init__.py @@ -8,12 +8,12 @@ import functools import threading from cereal.messaging import PubMaster from cereal.services import SERVICE_LIST -from openpilot.common.mock.generators import generate_liveLocationKalman +from openpilot.common.mock.generators import generate_livePose from openpilot.common.realtime import Ratekeeper MOCK_GENERATOR = { - "liveLocationKalman": generate_liveLocationKalman + "livePose": generate_livePose } diff --git a/common/mock/generators.py b/common/mock/generators.py index 40951faf85..5cd9c88a56 100644 --- a/common/mock/generators.py +++ b/common/mock/generators.py @@ -1,20 +1,14 @@ from cereal import messaging -LOCATION1 = (32.7174, -117.16277) -LOCATION2 = (32.7558, -117.2037) - -LLK_DECIMATION = 10 -RENDER_FRAMES = 15 -DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION - - -def generate_liveLocationKalman(location=LOCATION1): - msg = messaging.new_message('liveLocationKalman') - msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True} - msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} - msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} - msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} - msg.liveLocationKalman.status = 'valid' - msg.liveLocationKalman.gpsOK = True +def generate_livePose(): + msg = messaging.new_message('livePose') + meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True} + msg.livePose.orientationNED = meas + msg.livePose.velocityDevice = meas + msg.livePose.angularVelocityDevice = meas + msg.livePose.accelerationDevice = meas + msg.livePose.inputsOK = True + msg.livePose.posenetOK = True + msg.livePose.sensorsOK = True return msg diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4dd60faa06..6916161307 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -28,6 +28,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.system.hardware import HARDWARE @@ -78,6 +79,11 @@ class Controls: # Setup sockets self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents']) + if self.params.get_bool("UbloxAvailable"): + self.gps_location_service = "gpsLocationExternal" + else: + self.gps_location_service = "gpsLocation" + self.gps_packets = [self.gps_location_service] self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] @@ -86,16 +92,16 @@ class Controls: # TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + ['testJoystick'] + ignore = self.sensor_packets + self.gps_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick'] + self.camera_packets + self.sensor_packets, + 'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) @@ -120,6 +126,9 @@ class Controls: self.AM = AlertManager() self.events = Events() + self.pose_calibrator = PoseCalibrator() + self.calibrated_pose: Pose|None = None + self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) @@ -335,11 +344,9 @@ class Controls: self.logged_comm_issue = None if not (self.CP.notCar and self.joystick_mode): - if not self.sm['liveLocationKalman'].posenetOK: + if not self.sm['livePose'].posenetOK: self.events.add(EventName.posenetInvalid) - if not self.sm['liveLocationKalman'].deviceStable: - self.events.add(EventName.deviceFalling) - if not self.sm['liveLocationKalman'].inputsOK: + if not self.sm['livePose'].inputsOK: self.events.add(EventName.locationdTemporaryError) if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) @@ -375,10 +382,10 @@ class Controls: # TODO: fix simulator if not SIMULATION or REPLAY: - # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes - if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500): + 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): self.events.add(EventName.noGps) - if self.sm['liveLocationKalman'].gpsOK: + if gps_ok: self.distance_traveled = 0 self.distance_traveled += CS.vEgo * DT_CTRL @@ -429,6 +436,13 @@ class Controls: if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 + # calibrate the live pose and save it for later use + 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) + return CS def state_transition(self, CS): @@ -573,7 +587,7 @@ class Controls: actuators.curvature = self.desired_curvature actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited, self.desired_curvature, - self.sm['liveLocationKalman']) + self.calibrated_pose) # TODO what if not available else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.recv_frame['testJoystick'] > 0: @@ -650,12 +664,9 @@ class Controls: # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller - orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) - if len(orientation_value) > 2: - CC.orientationNED = orientation_value - angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value) - if len(angular_rate_value) > 2: - CC.angularVelocity = angular_rate_value + if self.calibrated_pose is not None: + CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() + CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index fddb331ccb..0e4a27da61 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,7 +17,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 329c486eb9..d3da656daa 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 9e6160838b..5b29958b69 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index decff2ec05..f4f56752bc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,7 +37,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -49,8 +49,9 @@ class LatControlTorque(LatControl): actual_curvature = actual_curvature_vm curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) else: - actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo - actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + assert calibrated_pose is not None + actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo + actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) curvature_deadzone = 0.0 desired_lateral_accel = desired_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index e12da6ed97..4835aa2349 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,7 +9,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.common.mock.generators import generate_liveLocationKalman +from openpilot.selfdrive.locationd.helpers import Pose +from openpilot.common.mock.generators import generate_livePose class TestLatControl: @@ -29,8 +30,10 @@ class TestLatControl: params = log.LiveParametersData.new_message() - llk = generate_liveLocationKalman() + lp = generate_livePose() + pose = Pose.from_live_pose(lp.livePose) + for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, llk) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose) assert lac_log.saturated diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 93b0430c1e..6222daf664 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -54,7 +54,7 @@ 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', 'roadCameraState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState'] + cameras) pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index af8a3d3726..af307b0336 100644 --- a/selfdrive/locationd/helpers.py +++ b/selfdrive/locationd/helpers.py @@ -2,6 +2,7 @@ import numpy as np from typing import Any from cereal import log +from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot def rotate_cov(rot_matrix, cov_in): @@ -70,3 +71,69 @@ class ParameterEstimator: def get_msg(self, valid: bool, with_points: bool) -> log.Event: raise NotImplementedError + + +class Measurement: + x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2])) + x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2])) + roll, pitch, yaw = x, y, z + roll_std, pitch_std, yaw_std = x_std, y_std, z_std + + def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray): + self.xyz: np.ndarray = xyz + self.xyz_std: np.ndarray = xyz_std + + @classmethod + def from_measurement_xyz(cls, measurement: log.LivePose.XYZMeasurement) -> 'Measurement': + return cls( + xyz=np.array([measurement.x, measurement.y, measurement.z]), + xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd]) + ) + + +class Pose: + def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement): + self.orientation = orientation + self.velocity = velocity + self.acceleration = acceleration + self.angular_velocity = angular_velocity + + @classmethod + def from_live_pose(cls, live_pose: log.LivePose) -> '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) + ) + + +class PoseCalibrator: + def __init__(self): + self.calib_valid = False + self.calib_from_device = np.eye(3) + + def _transform_calib_from_device(self, meas: Measurement): + new_xyz = self.calib_from_device @ meas.xyz + new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std) + return Measurement(new_xyz, new_xyz_std) + + def _ned_from_calib(self, orientation: Measurement): + ned_from_device = rot_from_euler(orientation.xyz) + ned_from_calib = ned_from_device @ self.calib_from_device.T + ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan)) + return ned_from_calib_euler_meas + + def build_calibrated_pose(self, pose: Pose) -> Pose: + ned_from_calib_euler = self._ned_from_calib(pose.orientation) + angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity) + acceleration_calib = self._transform_calib_from_device(pose.acceleration) + velocity_calib = self._transform_calib_from_device(pose.angular_velocity) + + 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) + 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 diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 1d5749cf34..459021b922 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -9,10 +9,9 @@ from cereal import car, log from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.numpy_fast import clip -from openpilot.common.transformations.orientation import rot_from_euler from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR -from openpilot.selfdrive.locationd.helpers import rotate_std +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.common.swaglog import cloudlog @@ -40,9 +39,8 @@ class ParamsLearner: self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) self.active = False - self.calibrated = False - self.calib_from_device = np.eye(3) + self.calibrator = PoseCalibrator() self.speed = 0.0 self.yaw_rate = 0.0 @@ -53,15 +51,12 @@ class ParamsLearner: def handle_log(self, t, which, msg): if which == 'livePose': - angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z]) - angular_velocity_device_std = np.array([msg.angularVelocityDevice.xStd, msg.angularVelocityDevice.yStd, msg.angularVelocityDevice.zStd]) - angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device) - angular_velocity_calibrated_std = rotate_std(self.calib_from_device, angular_velocity_device_std) + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std - self.yaw_rate, self.yaw_rate_std = angular_velocity_calibrated[2], angular_velocity_calibrated_std[2] - - localizer_roll = msg.orientationNED.x - localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.xStd) else msg.orientationNED.xStd + localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std + localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK if self.roll_valid: roll = localizer_roll @@ -73,7 +68,7 @@ class ParamsLearner: roll_std = np.radians(10.0) self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) - yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrated + yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s @@ -101,9 +96,7 @@ class ParamsLearner: self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) elif which == 'liveCalibration': - self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated - device_from_calib = rot_from_euler(np.array(msg.rpyCalib)) - self.calib_from_device = device_from_calib.T + self.calibrator.feed_live_calib(msg) elif which == 'carState': self.steering_angle = msg.steeringAngleDeg diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index edcf42ebcd..2b7cc62527 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -8,9 +8,8 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog -from openpilot.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 +from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -78,7 +77,7 @@ class TorqueEstimator(ParameterEstimator): self.offline_friction = CP.lateralTuning.torque.friction self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor - self.calib_from_device = np.eye(3) + self.calibrator = PoseCalibrator() self.reset() @@ -175,17 +174,17 @@ class TorqueEstimator(ParameterEstimator): 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 + self.calibrator.feed_live_calib(msg) # calculate lateral accel from past steering torque elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: - 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) + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + angular_velocity_calibrated = calibrated_pose.angular_velocity - yaw_rate = angular_velocity_calibrated[2] - roll = msg.orientationNED.x + yaw_rate = angular_velocity_calibrated.yaw + roll = device_pose.orientation.roll # check lat active up to now (without lag compensation) lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8cf7d4138a..170dbd0c82 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -465,6 +465,11 @@ def controlsd_config_callback(params, cfg, lr): assert controlsState is not None and initialized, "controlsState never initialized" params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) + ublox = params.get_bool("UbloxAvailable") + sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) + + cfg.pubs = set(cfg.pubs) - sub_keys + def locationd_config_pubsub_callback(params, cfg, lr): ublox = params.get_bool("UbloxAvailable") @@ -478,9 +483,10 @@ CONFIGS = [ proc_name="controlsd", pubs=[ "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", - "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState", + "longitudinalPlan", "livePose", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", - "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput" + "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput", + "gpsLocationExternal", "gpsLocation", ], subs=["controlsState", "carControl", "onroadEvents"], ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a85e8919c4..0d83bf96d1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6ca7313b9c1337fad1334d9e087cb4984fdce74d \ No newline at end of file +d768496a1a85bfe5b74c99a79203affdf9a0a065 \ No newline at end of file diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index aa297269d1..4919b6e6b3 100644 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -95,7 +95,7 @@ class TestPowerDraw: return now, msg_counts, time.monotonic() - start_time - SAMPLE_TIME - @mock_messages(['liveLocationKalman']) + @mock_messages(['livePose']) def test_camera_procs(self, subtests): baseline = get_power()