Std unit conversions (commaai#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
dragonpilot-community · Jun 4, 2018 · 0fe3e51 · 0fe3e51
1 parent 2f64198
commit 0fe3e51
Show file tree

Hide file tree

Showing 7 changed files with 27 additions and 49 deletions.
diff --git 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
@@ -155,7 +155,7 @@ def get_params(candidate, fingerprint):
 
     # 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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
     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
@@ -61,7 +61,7 @@ def get_params(candidate, fingerprint):
 
     # 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 @@ def get_params(candidate, fingerprint):
       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,23 +86,23 @@ def get_params(candidate, fingerprint):
       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.
     elif candidate == CAR.COROLLA:
       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.
     elif candidate == CAR.LEXUS_RXH:
       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
@@ -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
@@ -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
@@ -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
@@ -9,7 +9,8 @@
 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