Revert "Road Roll Compensation Rebased (commaai#23251)"

This reverts commit c94b745.

selfdrive/car/ford/carcontroller.py

@@ -49,7 +49,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel, dragon
 
     if (frame % 3) == 0:
 
-      curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
+      curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
 
       # The use of the toggle below is handy for trying out the various LKAS modes
       if TOGGLE_DEBUG:

selfdrive/car/honda/interface.py

@@ -418,6 +418,7 @@ def update(self, c, can_strings, dragonconf):
     ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
     ret.lkMode = self.CS.lkMode
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
+    ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
 
     #dp
     ret.engineRPM = self.CS.engineRPM

selfdrive/controls/controlsd.py

@@ -650,9 +650,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
 
     # Curvature & Steering angle
     params = self.sm['liveParameters']
-
-    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
-    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
+    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
 
     # controlsState
     dat = messaging.new_message('controlsState')

selfdrive/controls/lib/latcontrol_angle.py

@@ -18,7 +18,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
       angle_steers_des = float(CS.steeringAngleDeg)
     else:
       angle_log.active = True
-      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
       angle_steers_des += params.angleOffsetDeg
 
     angle_log.saturated = False

selfdrive/controls/lib/latcontrol_indi.py

@@ -93,11 +93,11 @@ def update(self, active, CS, CP, VM, params, curvature, curvature_rate):
     indi_log.steeringRateDeg = math.degrees(self.x[1])
     indi_log.steeringAccelDeg = math.degrees(self.x[2])
 
-    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
+    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
     steers_des += math.radians(params.angleOffsetDeg)
     indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
 
-    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
+    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
     indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
 
     if CS.vEgo < 0.3 or not active:

selfdrive/controls/lib/latcontrol_lqr.py

@@ -53,7 +53,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
     # Subtract offset. Zero angle should correspond to zero torque
     steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
 
-    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
 
     instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
     desired_angle += instant_offset  # Only add offset that originates from vehicle model errors

selfdrive/controls/lib/latcontrol_pid.py

@@ -21,7 +21,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
-    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des 

selfdrive/controls/lib/vehicle_model.py

@@ -5,7 +5,7 @@
 The state is x = [v, r]^T
 with v lateral speed [m/s], and r rotational speed [rad/s]
 
-The input u is the steering angle [rad], and roll [rad]
+The input u is the steering angle [rad]
 
 The system is defined by
 x_dot = A*x + B*u
@@ -20,9 +20,6 @@
 from cereal import car
 from common.params import Params
 
-ACCELERATION_DUE_TO_GRAVITY = 9.8
-
-
 class VehicleModel:
   def __init__(self, CP: car.CarParams):
     """
@@ -47,7 +44,7 @@ def update_params(self, stiffness_factor: float, steer_ratio: float) -> None:
     self.cR = stiffness_factor * self.cR_orig
     self.sR = steer_ratio
 
-  def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray:
+  def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
     """Returns the steady state solution.
 
     If the speed is too low we can't use the dynamic model (tire slip is undefined),
@@ -56,28 +53,26 @@ def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray:
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
-      roll: Road Roll [rad]
 
     Returns:
       2x1 matrix with steady state solution (lateral speed, rotational speed)
     """
     if u > 0.1:
-      return dyn_ss_sol(sa, u, roll, self)
+      return dyn_ss_sol(sa, u, self)
     else:
       return kin_ss_sol(sa, u, self)
 
-  def calc_curvature(self, sa: float, u: float, roll: float) -> float:
+  def calc_curvature(self, sa: float, u: float) -> float:
     """Returns the curvature. Multiplied by the speed this will give the yaw rate.
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
-      roll: Road Roll [rad]
 
     Returns:
       Curvature factor [1/m]
     """
-    return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u)
+    return self.curvature_factor(u) * sa / self.sR
 
   def curvature_factor(self, u: float) -> float:
     """Returns the curvature factor.
@@ -92,63 +87,43 @@ def curvature_factor(self, u: float) -> float:
     sf = calc_slip_factor(self)
     return (1. - self.chi) / (1. - sf * u**2) / self.l
 
-  def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float:
+  def get_steer_from_curvature(self, curv: float, u: float) -> float:
     """Calculates the required steering wheel angle for a given curvature
 
     Args:
       curv: Desired curvature [1/m]
       u: Speed [m/s]
-      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
 
-    return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u)
-
-  def roll_compensation(self, roll: float, u: float) -> float:
-    """Calculates the roll-compensation to curvature
-
-    Args:
-      roll: Road Roll [rad]
-      u: Speed [m/s]
+    return curv * self.sR * 1.0 / self.curvature_factor(u)
 
-    Returns:
-      Roll compensation curvature [rad]
-    """
-    sf = calc_slip_factor(self)
-
-    if abs(sf) < 1e-6:
-      return 0
-    else:
-      return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2)
-
-  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float:
+  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float) -> float:
     """Calculates the required steering wheel angle for a given yaw_rate
 
     Args:
       yaw_rate: Desired yaw rate [rad/s]
       u: Speed [m/s]
-      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
     curv = yaw_rate / u
-    return self.get_steer_from_curvature(curv, u, roll)
+    return self.get_steer_from_curvature(curv, u)
 
-  def yaw_rate(self, sa: float, u: float, roll: float) -> float:
+  def yaw_rate(self, sa: float, u: float) -> float:
     """Calculate yaw rate
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
-      roll: Road Roll [rad]
 
     Returns:
       Yaw rate [rad/s]
     """
-    return self.calc_curvature(sa, u, roll) * u
+    return self.calc_curvature(sa, u) * u
 
 
 def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
@@ -178,7 +153,7 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     VM: Vehicle model
 
   Returns:
-    A tuple with the 2x2 A matrix, and 2x2 B matrix
+    A tuple with the 2x2 A matrix, and 2x1 B matrix
 
   Parameters in the vehicle model:
     cF: Tire stiffness Front [N/rad]
@@ -191,38 +166,30 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     chi: Steer ratio rear [-]
   """
   A = np.zeros((2, 2))
-  B = np.zeros((2, 2))
+  B = np.zeros((2, 1))
   A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
   A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
   A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
   A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
-
-  # Steering input
   B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
   B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
-
-  # Roll input
-  B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY
-
   return A, B
 
 
-def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray:
+def dyn_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
   """Calculate the steady state solution when x_dot = 0,
   Ax + Bu = 0 => x = -A^{-1} B u
 
   Args:
     sa: Steering angle [rad]
     u: Speed [m/s]
-    roll: Road Roll [rad]
     VM: Vehicle model
 
   Returns:
     2x1 matrix with steady state solution
   """
   A, B = create_dyn_state_matrices(u, VM)
-  inp = np.array([[sa], [roll]])
-  return -solve(A, B) @ inp
+  return -solve(A, B) * sa
 
 
 def calc_slip_factor(VM):