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
pull/260/head
dekerr 7 years ago committed by rbiasini
parent f49e9f4f09
commit 8849aa02a3
  1. 25
      common/numpy_fast.py
  2. 14
      selfdrive/car/honda/interface.py
  3. 10
      selfdrive/car/toyota/interface.py
  4. 19
      selfdrive/config.py
  5. 3
      selfdrive/controls/lib/planner.py
  6. 2
      selfdrive/test/plant/plant_ui.py
  7. 3
      selfdrive/test/tests/plant/test_longitudinal.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)

@ -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

@ -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

@ -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

@ -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)

@ -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

@ -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

Loading…
Cancel
Save