apply roll+grade compensation per commaai#30341

selfdrive/controls/lib/latcontrol_torque.py

@@ -55,6 +55,9 @@ def get_lookahead_value(future_vals, current_val):
   min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
   return min_val
 
+def roll_grade_adjust(roll, pitch):
+  return np.sin(roll) * np.cos(pitch)
+
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
@@ -132,6 +135,8 @@ def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_
         # update past data
         error = setpoint - measurement
         roll = params.roll
+        pitch = llk.orientationNED.value[1]
+        roll = roll_grade_adjust(roll, pitch)
         self.roll_deque.append(roll)
         self.lateral_accel_desired_deque.append(desired_lateral_accel)
         self.error_deque.append(error)
@@ -146,7 +151,7 @@ def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_
         # adjust future times to account for longitudinal acceleration
         adjusted_future_times = [t + 0.5*CS.aEgo*(t/max(CS.vEgo, 1.0)) for t in self.nn_future_times]
         past_rolls = [self.roll_deque[min(len(self.roll_deque)-1, i)] for i in self.history_frame_offsets]
-        future_rolls = [interp(t, T_IDXS, model_data.orientation.x) + roll for t in adjusted_future_times]
+        future_rolls = [roll_grade_adjust(interp(t, T_IDXS, model_data.orientation.x) + roll, interp(t, T_IDXS, model_data.orientation.y) + pitch) for t in adjusted_future_times]
         past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, i)] for i in self.history_frame_offsets]
         future_planned_lateral_accels = [interp(t, T_IDXS[:CONTROL_N], lat_plan.curvatures) * CS.vEgo ** 2 for t in adjusted_future_times]
         past_errors = [self.error_deque[min(len(self.error_deque)-1, i)] for i in self.history_frame_offsets]