diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index dffe85c473..5a2814e089 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -52,10 +52,8 @@ class LatControlTorque(LatControl): actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 @@ -67,6 +65,8 @@ class LatControlTorque(LatControl): # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel + # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll + ff -= self.torque_params.latAccelOffset ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 diff --git a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py new file mode 100644 index 0000000000..5ba9980020 --- /dev/null +++ b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py @@ -0,0 +1,71 @@ +import numpy as np +from cereal import car, messaging +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY +from opendbc.car import structs +from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS + +np.random.seed(0) + +LA_ERR_STD = 1.0 +INPUT_NOISE_STD = 0.1 +V_EGO = 30.0 + +WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int) +STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03) + +ROLL_BIAS_DEG = 1.0 +ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG))) +TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2) +TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2) + + +def generate_inputs(torque_tune, la_err_std, input_noise_std=None): + rng = np.random.default_rng(0) + steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)]) + la_errs = rng.normal(scale=la_err_std, size=steer_torques.size) + frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs]) + lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions + if input_noise_std is not None: + steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size) + lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size) + return steer_torques, lat_accels + +def get_warmed_up_estimator(steer_torques, lat_accels): + est = TorqueEstimator(car.CarParams()) + for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True): + est.filtered_points.add_point(steer_torque, lat_accel) + return est + +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 + carControl.latActive = True + carState.vEgo = V_EGO + carState.steeringPressed = False + ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET) + steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET))) + 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)) + livePose.angularVelocityDevice.z = float(lat_accel / V_EGO) + for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)): + est.handle_log(t, which, msg) + +def test_estimated_offset(): + steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) + 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.03 + +def test_straight_road_roll_bias(): + 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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8e4ff1e2a8..7d21526144 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -209b47bea61e145cf2d27eb3ab650c97bcd1d33f \ No newline at end of file +866f2cb1f0e49b2cb7115aa8131164b3a75fb2c5 \ No newline at end of file