mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Std unit conversions (#259)

Browse Source 
* 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

old-commit-hash: 8849aa02a3e0acefb53fda482aa0f9ae97cc0ef0
This commit is contained in:
dekerr
2018-06-04 15:39:54 -04:00
committed by rbiasini
parent 723d9d8b70
commit d8318833a6
7 changed files with 27 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
common/numpy_fast.py
View File

@@ -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__'):
def get_interp(xv):
hi = 0
while hi < N and x > xp[hi]:
while hi < N and xv > 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:
hi = 0
while hi < N and v > 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)

14
selfdrive/car/honda/interface.py
View File

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

10
selfdrive/car/toyota/interface.py
View File

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

19
selfdrive/config.py
View File

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

3
selfdrive/controls/lib/planner.py
View File

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

2
selfdrive/test/plant/plant_ui.py
View File

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

3
selfdrive/test/tests/plant/test_longitudinal.py
View File

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