From 94b0fd629e3db1740df6b8f0b765d83aa1d1dd29 Mon Sep 17 00:00:00 2001 From: qadmus <42746943+qadmus@users.noreply.github.com> Date: Mon, 28 Sep 2020 02:54:48 -0700 Subject: [PATCH] Fix grade force in test_long plant (#2225) Co-authored-by: user old-commit-hash: cba53957bd33379ff6ad9fc53dd5edc73cfdff4a --- selfdrive/test/longitudinal_maneuvers/plant.py | 6 +++--- .../test/longitudinal_maneuvers/test_longitudinal.py | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 9acb9cd282..dffda44068 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 24f5015bd2..6c57f5fc3f 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/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], ),