apply roll+grade compensation per commaai#30341

selfdrive/controls/lib/latcontrol_torque.py

@@ -55,6 +55,15 @@ 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):
+  # calculate grade from pitch
+  grade = np.tan(pitch)
+  sin_theta = roll / (1. + roll**2)**0.5
+  cos_phi = 1. / (1. + grade**2)**0.5
+  theta = np.arcsin(sin_theta)
+  a = sin_theta * cos_phi
+  return a
+
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
@@ -132,6 +141,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 +157,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]