From 8849aa02a3e0acefb53fda482aa0f9ae97cc0ef0 Mon Sep 17 00:00:00 2001 From: dekerr Date: Mon, 4 Jun 2018 15:39:54 -0400 Subject: [PATCH] Std unit conversions (#259) * Added conversion constants * implemented std unit conversion * changed centerToFront ratio Changed weight distribution ratios used to calc center of gravity distances to align closer to manufacturer specs * implemented std unit conversion * remove unused conversion * reverted wheelbase conversion slight change to pilot wheelbase * removed redundant conversion * removed incorrect/unused conversion * removed class that now exists in honda/values.py * redirect Cruisebuttons call * redirect Cruisebuttons call * Update interface.py * Update numpy_fast.py Refactor * Update numpy_fast.py * Update numpy_fast.py -encapsulated get_interp -reduced calls to len() for iterable input --- common/numpy_fast.py | 25 ++++++------------- selfdrive/car/honda/interface.py | 14 +++++------ selfdrive/car/toyota/interface.py | 10 ++++---- selfdrive/config.py | 19 +++----------- selfdrive/controls/lib/planner.py | 3 +-- selfdrive/test/plant/plant_ui.py | 2 +- .../test/tests/plant/test_longitudinal.py | 3 ++- 7 files changed, 27 insertions(+), 49 deletions(-) diff --git a/common/numpy_fast.py b/common/numpy_fast.py index a71757d32f..eb706a908f 100644 --- a/common/numpy_fast.py +++ b/common/numpy_fast.py @@ -1,29 +1,18 @@ def int_rnd(x): return int(round(x)) - def clip(x, lo, hi): return max(lo, min(hi, x)) - def interp(x, xp, fp): N = len(xp) - if not hasattr(x, '__iter__'): - hi = 0 - while hi < N and x > xp[hi]: - hi += 1 - low = hi - 1 - return fp[-1] if hi == N and x > xp[low] else ( - fp[0] if hi == 0 else - (x - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) - - result = [] - for v in x: + def get_interp(xv): hi = 0 - while hi < N and v > xp[hi]: + while hi < N and xv > xp[hi]: hi += 1 low = hi - 1 - result.append(fp[-1] if hi == N and v > xp[low] else (fp[ - 0] if hi == 0 else (v - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low] - ) + fp[low])) - return result + return fp[-1] if hi == N and xv > xp[low] else ( + fp[0] if hi == 0 else + (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) + return [get_interp(v) for v in x] if hasattr( + x, '__iter__') else get_interp(x) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 7beaa6b5b8..c50c0de9ed 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -155,7 +155,7 @@ class CarInterface(object): # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars - mass_civic = 2923./2.205 + std_cargo + mass_civic = 2923 * CV.LB_TO_KG + std_cargo wheelbase_civic = 2.70 centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic @@ -180,7 +180,7 @@ class CarInterface(object): ret.longitudinalKiV = [0.54, 0.36] elif candidate == CAR.ACURA_ILX: stop_and_go = False - ret.mass = 3095./2.205 + std_cargo + ret.mass = 3095 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 15.3 @@ -194,7 +194,7 @@ class CarInterface(object): ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.CRV: stop_and_go = False - ret.mass = 3572./2.205 + std_cargo + ret.mass = 3572 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 @@ -206,7 +206,7 @@ class CarInterface(object): ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ACURA_RDX: stop_and_go = False - ret.mass = 3935./2.205 + std_cargo + ret.mass = 3935 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 @@ -218,7 +218,7 @@ class CarInterface(object): ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False - ret.mass = 4354./2.205 + std_cargo + ret.mass = 4354 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 @@ -230,7 +230,7 @@ class CarInterface(object): ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.PILOT: stop_and_go = False - ret.mass = 4303./2.205 + std_cargo + ret.mass = 4303 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 @@ -243,7 +243,7 @@ class CarInterface(object): elif candidate == CAR.RIDGELINE: stop_and_go = False ts_factor = 1.4 - ret.mass = 4515./2.205 + std_cargo + ret.mass = 4515 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 55310d7ab7..0d6cf6d26f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -61,7 +61,7 @@ class CarInterface(object): # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars - mass_civic = 2923./2.205 + std_cargo + mass_civic = 2923 * CV.LB_TO_KG + std_cargo wheelbase_civic = 2.70 centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic @@ -74,7 +74,7 @@ class CarInterface(object): ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.0 - ret.mass = 3045./2.205 + std_cargo + ret.mass = 3045 * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.5 @@ -86,7 +86,7 @@ class CarInterface(object): ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 ret.steerRatio = 14.5 # Rav4 2017 - ret.mass = 3650./2.205 + std_cargo # mean between normal and hybrid + ret.mass = 3650 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. @@ -94,7 +94,7 @@ class CarInterface(object): ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.8 - ret.mass = 2860./2.205 + std_cargo # mean between normal and hybrid + ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. @@ -102,7 +102,7 @@ class CarInterface(object): ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.79 ret.steerRatio = 16. # official specs say 14.8, but it does not seem right - ret.mass = 4481./2.205 + std_cargo # mean between min and max + ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = .8 diff --git a/selfdrive/config.py b/selfdrive/config.py index 3488a43816..1219965e11 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -1,6 +1,7 @@ import numpy as np class Conversions: + #Speed MPH_TO_KPH = 1.609344 KPH_TO_MPH = 1. / MPH_TO_KPH MS_TO_KPH = 3.6 @@ -9,24 +10,12 @@ class Conversions: MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS MS_TO_KNOTS = 1.9438 KNOTS_TO_MS = 1. / MS_TO_KNOTS + #Angle DEG_TO_RAD = np.pi/180. RAD_TO_DEG = 1. / DEG_TO_RAD + #Mass + LB_TO_KG = 0.453592 - # Car decode decimal minutes into decimal degrees, can work with numpy arrays as input - @staticmethod - def dm2d(dm): - degs = np.round(dm/100.) - mins = dm - degs*100. - return degs + mins/60. - - -# Car button codes -# TODO: this is Honda specific, move to honda/interface.py -class CruiseButtons: - RES_ACCEL = 4 - DECEL_SET = 3 - CANCEL = 2 - MAIN = 1 RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 24ae0f29a0..5beec543e7 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -64,10 +64,9 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): This function returns a limited long acceleration allowed, depending on the existing lateral acceleration this should avoid accelerating when losing the target in turns """ - deg_to_rad = np.pi / 180. # from can reading to rad a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) - a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.steerRatio * CP.wheelbase) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) a_target[1] = min(a_target[1], a_x_allowed) diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/plant/plant_ui.py index 5cc72ecf8c..d7b4bf1856 100755 --- a/selfdrive/test/plant/plant_ui.py +++ b/selfdrive/test/plant/plant_ui.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import pygame from plant import Plant -from selfdrive.config import CruiseButtons +from selfdrive.car.honda.values import CruiseButtons import numpy as np import selfdrive.messaging as messaging import math diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index e6f4a22507..e5affb6276 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -9,7 +9,8 @@ import shutil import matplotlib matplotlib.use('svg') -from selfdrive.config import Conversions as CV, CruiseButtons as CB +from selfdrive.config import Conversions as CV +from selfdrive.car.honda.values import CruiseButtons as CB from selfdrive.test.plant.maneuver import Maneuver import selfdrive.manager as manager from common.params import Params