Fix grade force in test_long plant (#2225)

Co-authored-by: user <email@web.com>
old-commit-hash: cba53957bd

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -49,7 +49,7 @@ def car_plant(pos, speed, grade, gas, brake):
   power_peak = 100000   # 100kW
   speed_base = power_peak/force_peak
   rolling_res = 0.01
-  g = 9.81
+  gravity = 9.81
   frontal_area = 2.2
   air_density = 1.225
   gas_to_peak_linear_slope = 3.33
@@ -65,12 +65,12 @@ def car_plant(pos, speed, grade, gas, brake):
   else:  # power control
     force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
 
-  force_grade = - grade * mass  # positive grade means uphill
+  force_grade = - np.sin(np.arctan(grade)) * mass * gravity
 
   creep_accel = np.interp(speed, creep_accel_bp, creep_accel_v)
   force_creep = creep_accel * mass
 
-  force_resistance = -(rolling_res * mass * g + 0.5 * speed**2 * aero_cd * air_density * frontal_area)
+  force_resistance = -(rolling_res * mass * gravity + 0.5 * speed**2 * aero_cd * air_density * frontal_area)
   force = force_gas + force_brake + force_resistance + force_grade + force_creep
   acceleration = force / mass
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -53,20 +53,20 @@ maneuvers = [
     checks=[check_engaged],
   ),
   Maneuver(
-    'while cruising at 20mph, grade change +10%',
+    'while cruising at 20mph, uphill grade of 10%',
     duration=25.,
     initial_speed=20. * CV.MPH_TO_MS,
     cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
-    grade_values=[0., 0., 1.0],
+    grade_values=[0., 0., .1],
     grade_breakpoints=[0., 10., 11.],
     checks=[check_engaged],
   ),
   Maneuver(
-    'while cruising at 20mph, grade change -10%',
+    'while cruising at 20mph, downhill grade of -10%',
     duration=25.,
     initial_speed=20. * CV.MPH_TO_MS,
     cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
-    grade_values=[0., 0., -1.0],
+    grade_values=[0., 0., -.1],
     grade_breakpoints=[0., 10., 11.],
     checks=[check_engaged],
   ),