sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.9 tweaks

Browse Source
pull/154/head
Vehicle Researcher 7 years ago
parent 5627d0d7fd
commit 1ad9cc8c67
30 changed files with 472 additions and 281 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      README.md
  2. BIN
      apk/com.baseui.apk
  3. 70
      cereal/car.capnp
  4. 2
      cereal/log.capnp
  5. 16
      common/kalman/ekf.py
  6. 23
      common/kalman/simple_kalman.py
  7. 1
      common/params.py
  8. 2
      opendbc
  9. 6
      selfdrive/car/honda/carcontroller.py
  10. 71
      selfdrive/car/honda/interface.py
  11. 37
      selfdrive/car/toyota/interface.py
  12. 2
      selfdrive/controls/controlsd.py
  13. 3
      selfdrive/controls/lib/drive_helpers.py
  14. 23
      selfdrive/controls/lib/latcontrol.py
  15. 2
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  16. 12
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
  17. 150
      selfdrive/controls/lib/planner.py
  18. 96
      selfdrive/controls/lib/radar_helpers.py
  19. 6
      selfdrive/controls/lib/speed_smoother.py
  20. 59
      selfdrive/controls/lib/vehicle_model.py
  21. 47
      selfdrive/controls/radard.py
  22. 7
      selfdrive/manager.py
  23. 2
      selfdrive/radar/nidec/interface.py
  24. 22
      selfdrive/radar/toyota/interface.py
  25. 5
      selfdrive/test/plant/maneuver.py
  26. 8
      selfdrive/test/plant/maneuverplots.py
  27. 14
      selfdrive/test/plant/plant.py
  28. 43
      selfdrive/test/tests/plant/test_longitudinal.py
  29. 20
      selfdrive/ui/ui.c
  30. BIN
      selfdrive/visiond/visiond

4
README.md
Unescape View File

@ -31,11 +31,11 @@ Supported Cars
- Toyota RAV-4 2016+ with TSS-P (alpha!)
- By default it uses stock Toyota ACC for longitudinal control
- openpilot longitudinal control available after unplugging the Driving Support ECU and can be enabled above 20 mph
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) and can be enabled above 20 mph
- Toyota Prius 2017 (alpha!)
- By default it uses stock Toyota ACC for longitudinal control
- openpilot longitudinal control available after unplugging the Driving Support ECU
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29)
In Progress Cars
------

BIN
apk/com.baseui.apk
View File

Binary file not shown.

70
cereal/car.capnp
Unescape View File

@ -173,6 +173,9 @@ struct RadarState {
# these are optional and valid if they are not NaN
aRel @4 :Float32; # m/s^2
yvRel @5 :Float32; # m/s
# some radars flag measurements VS estimates
measured @6 :Bool;
}
}
@ -252,22 +255,22 @@ struct CarParams {
enableGas @4 :Bool;
enableBrake @5 :Bool;
enableCruise @6 :Bool;
enableCamera @27 :Bool;
enableDsu @28 :Bool; # driving support unit
enableApgs @29 :Bool; # advanced parking guidance system
enableCamera @26 :Bool;
enableDsu @27 :Bool; # driving support unit
enableApgs @28 :Bool; # advanced parking guidance system
minEnableSpeed @18 :Float32;
safetyModel @19 :Int16;
minEnableSpeed @17 :Float32;
safetyModel @18 :Int16;
steerMaxBP @20 :List(Float32);
steerMaxV @21 :List(Float32);
gasMaxBP @22 :List(Float32);
gasMaxV @23 :List(Float32);
brakeMaxBP @24 :List(Float32);
brakeMaxV @25 :List(Float32);
steerMaxBP @19 :List(Float32);
steerMaxV @20 :List(Float32);
gasMaxBP @21 :List(Float32);
gasMaxV @22 :List(Float32);
brakeMaxBP @23 :List(Float32);
brakeMaxV @24 :List(Float32);
longPidDeadzoneBP @33 :List(Float32);
longPidDeadzoneV @34 :List(Float32);
longPidDeadzoneBP @32 :List(Float32);
longPidDeadzoneV @33 :List(Float32);
enum SafetyModels {
# does NOT match board setting
@ -278,33 +281,32 @@ struct CarParams {
}
# things about the car in the manual
m @7 :Float32; # [kg] running weight
l @8 :Float32; # [m] wheelbase
sR @9 :Float32; # [] steering ratio
aF @10 :Float32; # [m] GC distance to front axle
aR @11 :Float32; # [m] GC distance to rear axle
chi @12 :Float32; # [] rear steering ratio wrt front steering (usually 0)
mass @7 :Float32; # [kg] running weight
wheelbase @8 :Float32; # [m] distance from rear to front axle
centerToFront @9 :Float32; # [m] GC distance to front axle
steerRatio @10 :Float32; # [] ratio between front wheels and steering wheel angles
steerRatioRear @11 :Float32; # [] rear steering ratio wrt front steering (usually 0)
# things we can derive
j @13 :Float32; # [kg*m2] body rotational inertia
cF @14 :Float32; # [N/rad] front tire coeff of stiff
cR @15 :Float32; # [N/rad] rear tire coeff of stiff
rotationalInertia @12 :Float32; # [kg*m2] body rotational inertia
tireStiffnessFront @13 :Float32; # [N/rad] front tire coeff of stiff
tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff
# Kp and Ki for the lateral control
steerKp @16 :Float32;
steerKi @17 :Float32;
steerKf @26 :Float32;
steerKp @15 :Float32;
steerKi @16 :Float32;
steerKf @25 :Float32;
# Kp and Ki for the longitudinal control
longitudinalKpBP @37 :List(Float32);
longitudinalKpV @38 :List(Float32);
longitudinalKiBP @39 :List(Float32);
longitudinalKiV @40 :List(Float32);
longitudinalKpBP @36 :List(Float32);
longitudinalKpV @37 :List(Float32);
longitudinalKiBP @38 :List(Float32);
longitudinalKiV @39 :List(Float32);
steerLimitAlert @30 :Bool;
steerLimitAlert @29 :Bool;
vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state
directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake
stoppingControl @35 :Bool; # Does the car allows full control even at lows speeds when stopping
startAccel @36 :Float32; # Required acceleraton to overcome creep braking
vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
startAccel @35 :Float32; # Required acceleraton to overcome creep braking
}

2
cereal/log.capnp
Unescape View File

@ -287,7 +287,7 @@ struct Live20Data {
vRel @2 :Float32;
aRel @3 :Float32;
vLead @4 :Float32;
aLead @5 :Float32;
aLeadDEPRECATED @5 :Float32;
dPath @6 :Float32;
vLat @7 :Float32;
vLeadK @8 :Float32;

16
common/kalman/ekf.py
Unescape View File

@ -19,6 +19,7 @@ import numpy.matlib
# update() should be called once per sensor, and can be called multiple times between predict steps.
# Access and set the state of the filter directly with ekf.state and ekf.covar.
class SensorReading:
# Given a perfect model and no noise, data = obs_model * state
def __init__(self, data, covar, obs_model):
@ -33,7 +34,7 @@ class SensorReading:
# A generic sensor class that does no pre-processing of data
class SimpleSensor:
# obs_model can be
# obs_model can be
# a full obesrvation model matrix, or
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be
@ -131,11 +132,11 @@ class EKF:
# like update but knowing that measurment is a scalar
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state)
# innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
# kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
# self.state += np.matmul(kalman_gain, innovation)
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
# innovation = reading.data - np.matmul(reading.obs_model, self.state)
# innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
# kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
# self.state += np.matmul(kalman_gain, innovation)
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
# self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
# written without np.matmul
@ -174,7 +175,7 @@ class EKF:
#! Clip covariance to avoid explosions
self.covar = np.clip(self.covar,-1e10,1e10)
@abc.abstractmethod
def calc_transfer_fun(self, dt):
"""Return a tuple with the transfer function and transfer function jacobian
@ -190,6 +191,7 @@ class EKF:
and using it during calcualtion of A and J
"""
class FastEKF1D(EKF):
"""Fast version of EKF for 1D problems with scalar readings."""

23
common/kalman/simple_kalman.py
Unescape View File

@ -0,0 +1,23 @@
import numpy as np
class KF1D:
# this EKF assumes constant covariance matrix, so calculations are much simpler
# the Kalman gain also needs to be precomputed using the control module
def __init__(self, x0, A, C, K):
self.x = x0
self.A = A
self.C = C
self.K = K
self.A_K = self.A - np.dot(self.K, self.C)
# K matrix needs to be pre-computed as follow:
# import control
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
# self.K = np.transpose(K)
def update(self, meas):
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
return self.x

1
common/params.py
Unescape View File

@ -57,6 +57,7 @@ keys = {
"IsMetric": TxType.PERSISTANT,
"IsRearViewMirror": TxType.PERSISTANT,
"IsFcwEnabled": TxType.PERSISTANT,
"HasAcceptedTerms": TxType.PERSISTANT,
# written: visiond
# read: visiond, controlsd
"CalibrationParams": TxType.PERSISTANT,

2
opendbc

@ -1 +1 @@
Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21
Subproject commit 242698f80038bab677a4a6e58127309f9ed38d93

6
selfdrive/car/honda/carcontroller.py
Unescape View File

@ -11,10 +11,6 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from . import hondacan
from .values import AH
# msgs sent for steering controller by camera module on can 0.
# those messages are mutually exclusive on non-rav4 and rav4 cars
CAMERA_MSGS = [0xe4, 0x194]
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams
@ -126,7 +122,7 @@ class CarController(object):
GAS_MAX = 1004
BRAKE_MAX = 1024/4
if CS.civic:
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value

71
selfdrive/car/honda/interface.py
Unescape View File

@ -10,7 +10,6 @@ from cereal import car
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.car.honda.carstate import CarState, get_can_parser
from selfdrive.car.honda.carcontroller import CAMERA_MSGS
from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
from selfdrive.controls.lib.planner import A_ACC_MAX
@ -20,6 +19,11 @@ except ImportError:
CarController = None
# msgs sent for steering controller by camera module on can 0.
# those messages are mutually exclusive on CRV and non-CRV cars
CAMERA_MSGS = [0xe4, 0x194]
def compute_gb_honda(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
@ -108,7 +112,7 @@ class CarInterface(object):
def calc_accel_override(a_ego, a_target, v_ego, v_target):
eA = a_ego - a_target
valuesA = [1.0, 0.1]
bpA = [0.0, 0.5]
bpA = [0.3, 1.1]
eV = v_ego - v_target
valuesV = [1.0, 0.1]
@ -144,22 +148,22 @@ class CarInterface(object):
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
aF_civic = l_civic * 0.4
aR_civic = l_civic - aF_civic
j_civic = 2500
cF_civic = 85400
cR_civic = 90000
mass_civic = 2923./2.205 + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING":
stop_and_go = True
ret.m = m_civic
ret.l = l_civic
ret.aF = aF_civic
ret.sR = 13.0
ret.mass = mass_civic
ret.wheelbase = wheelbase_civic
ret.centerToFront = centerToFront_civic
ret.steerRatio = 13.0
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
ret.longitudinalKpBP = [0., 5., 35.]
@ -168,10 +172,10 @@ class CarInterface(object):
ret.longitudinalKiV = [0.54, 0.36]
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
stop_and_go = False
ret.m = 3095./2.205 + std_cargo
ret.l = 2.67
ret.aF = ret.l * 0.37
ret.sR = 15.3
ret.mass = 3095./2.205 + std_cargo
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.37
ret.steerRatio = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
@ -182,10 +186,10 @@ class CarInterface(object):
ret.longitudinalKiV = [0.18, 0.12]
elif candidate == "HONDA ACCORD 2016 TOURING":
stop_and_go = False
ret.m = 3580./2.205 + std_cargo
ret.l = 2.74
ret.aF = ret.l * 0.38
ret.sR = 15.3
ret.mass = 3580./2.205 + std_cargo
ret.wheelbase = 2.74
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
ret.longitudinalKpBP = [0., 5., 35.]
@ -194,10 +198,10 @@ class CarInterface(object):
ret.longitudinalKiV = [0.18, 0.12]
elif candidate == "HONDA CR-V 2016 TOURING":
stop_and_go = False
ret.m = 3572./2.205 + std_cargo
ret.l = 2.62
ret.aF = ret.l * 0.41
ret.sR = 15.3
ret.mass = 3572./2.205 + std_cargo
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
ret.longitudinalKpBP = [0., 5., 35.]
@ -214,18 +218,23 @@ class CarInterface(object):
# conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
ret.aR = ret.l - ret.aF
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
ret.tireStiffnessFront = tireStiffnessFront_civic * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = tireStiffnessRear_civic * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
ret.steerRatioRear = 0.
# no max steer limit VS speed
ret.steerMaxBP = [0.] # m/s

37
selfdrive/car/toyota/interface.py
Unescape View File

@ -62,19 +62,19 @@ class CarInterface(object):
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
aF_civic = l_civic * 0.4
aR_civic = l_civic - aF_civic
j_civic = 2500
cF_civic = 85400
cR_civic = 90000
mass_civic = 2923./2.205 + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
stop_and_go = True
ret.m = 3045./2.205 + std_cargo
ret.l = 2.70
ret.aF = ret.l * 0.44
ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius
ret.mass = 3045./2.205 + std_cargo
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.44
ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
ret.steerKp, ret.steerKi = 0.6, 0.05
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
@ -88,18 +88,23 @@ class CarInterface(object):
elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
ret.aR = ret.l - ret.aF
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
ret.tireStiffnessFront = tireStiffnessFront_civic * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = tireStiffnessRear_civic * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
ret.steerRatioRear = 0.
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph

2
selfdrive/controls/controlsd.py
Unescape View File

@ -476,7 +476,7 @@ def controlsd_thread(gctx, rate=100):
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
angle_offset = 1.5 # Default model bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:

3
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -42,7 +42,8 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, ang
min_learn_speed = 1.
# learn less at low speed or when turning
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.2 * abs(angle_steers))
slow_factor = 1. / (1. + 0.02 * abs(angle_steers) * v_ego)
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) * slow_factor
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:

23
selfdrive/controls/lib/latcontrol.py
Unescape View File

@ -1,11 +1,14 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import clip, interp
from common.numpy_fast import interp
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.1
ACTUATORS_DELAY = 0.2
_DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz
@ -24,6 +27,7 @@ def get_steer_max(CP, v_ego):
class LatControl(object):
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc()
def setup_mpc(self):
@ -61,7 +65,7 @@ class LatControl(object):
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@ -71,10 +75,21 @@ class LatControl(object):
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if nans:
self.libmpc.init()
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()

2
selfdrive/controls/lib/lateral_mpc/generator.cpp
Unescape View File

@ -79,7 +79,7 @@ int main( )
Q(3,3) = 1.0;
Q(4,4) = 2.0;
Q(4,4) = 1.0;
// Terminal cost
Function hN;

12
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
Unescape View File

@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0];
tmpQ2[1] = + tmpFx[4];
tmpQ2[2] = + tmpFx[8];
tmpQ2[3] = + tmpFx[12];
tmpQ2[4] = + tmpFx[16]*(real_t)2.0000000000000000e+00;
tmpQ2[4] = + tmpFx[16];
tmpQ2[5] = + tmpFx[1];
tmpQ2[6] = + tmpFx[5];
tmpQ2[7] = + tmpFx[9];
tmpQ2[8] = + tmpFx[13];
tmpQ2[9] = + tmpFx[17]*(real_t)2.0000000000000000e+00;
tmpQ2[9] = + tmpFx[17];
tmpQ2[10] = + tmpFx[2];
tmpQ2[11] = + tmpFx[6];
tmpQ2[12] = + tmpFx[10];
tmpQ2[13] = + tmpFx[14];
tmpQ2[14] = + tmpFx[18]*(real_t)2.0000000000000000e+00;
tmpQ2[14] = + tmpFx[18];
tmpQ2[15] = + tmpFx[3];
tmpQ2[16] = + tmpFx[7];
tmpQ2[17] = + tmpFx[11];
tmpQ2[18] = + tmpFx[15];
tmpQ2[19] = + tmpFx[19]*(real_t)2.0000000000000000e+00;
tmpQ2[19] = + tmpFx[19];
tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16];
tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17];
tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18];
@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0];
tmpR2[1] = + tmpFu[1];
tmpR2[2] = + tmpFu[2];
tmpR2[3] = + tmpFu[3];
tmpR2[4] = + tmpFu[4]*(real_t)2.0000000000000000e+00;
tmpR2[4] = + tmpFu[4];
tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4];
}
@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5];
tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1];
tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2];
tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3];
tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)2.0000000000000000e+00;
tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4];
objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4];
}

150
selfdrive/controls/lib/planner.py
Unescape View File

@ -3,6 +3,7 @@ import zmq
import numpy as np
import math
from collections import defaultdict
from common.realtime import sec_since_boot
from common.params import Params
@ -39,6 +40,9 @@ _A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
_FCW_A_ACT_V = [-3., -2.]
_FCW_A_ACT_BP = [0., 30.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
@ -61,7 +65,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
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.sR * CP.l)
a_y = v_ego**2 * angle_steers * 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)
@ -70,36 +74,64 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
class FCWChecker(object):
def __init__(self):
self.fcw_count = 0
self.last_fcw_a = 0.0
self.v_lead_max = 0.0
self.lead_seen_t = 0.0
self.last_fcw_time = 0.0
self.reset_lead(0.0)
def reset_lead(self, cur_time):
self.last_fcw_a = 0.0
self.v_lead_max = 0.0
self.lead_seen_t = cur_time
self.last_fcw_time = 0.0
self.last_min_a = 0.0
self.counters = defaultdict(lambda: 0)
def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers):
min_a_mpc = min(list(mpc_solution[0].a_ego)[1:])
@staticmethod
def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
max_ttc = 5.0
v_rel = v_ego - v_lead
a_rel = a_ego - a_lead
# assuming that closing gap ARel comes from lead vehicle decel,
# then limit ARel so that v_lead will get to zero in no sooner than t_decel.
# This helps underweighting ARel when v_lead is close to zero.
t_decel = 2.
a_rel = np.minimum(a_rel, v_lead/t_decel)
# delta of the quadratic equation to solve for ttc
delta = v_rel**2 + 2 * x_lead * a_rel
# assign an arbitrary high ttc value if there is no solution to ttc
if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
ttc = max_ttc
else:
ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
return ttc
def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
self.last_min_a = min(mpc_solution_a[1:])
self.v_lead_max = max(self.v_lead_max, v_lead)
if (fcw_lead > 0.99
and v_ego > 5.0
and min_a_mpc < -4.0
and self.v_lead_max > 2.5
and v_ego > v_lead
and self.lead_seen_t < cur_time - 2.0
and abs(y_lead) < 1.0
and abs(vlat_lead) < 0.3
and not blinkers):
self.fcw_count += 1
if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time:
if (fcw_lead > 0.99):
ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
a_delta = min(mpc_solution_a[1:15]) - min(0.0, a_ego)
fcw_allowed = all(c >= 10 for c in self.counters.values())
if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
self.last_fcw_time = cur_time
self.last_fcw_a = min_a_mpc
self.last_fcw_a = self.last_min_a
return True
else:
self.fcw_count = 0
return False
@ -214,6 +246,8 @@ class LongitudinalMpc(object):
self.libmpc.init()
self.cur_state[0].v_ego = CS.vEgo
self.cur_state[0].a_ego = 0.0
self.v_mpc = CS.vEgo
self.a_mpc = CS.aEgo
self.prev_lead_status = False
@ -257,32 +291,33 @@ class Planner(object):
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
def choose_solution(self, v_cruise_setpoint):
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
if _DEBUG:
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
if _DEBUG:
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
@ -331,19 +366,19 @@ class Planner(object):
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1],
jerk_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
self.v_cruise = CS.vEgo
self.a_cruise = self.CP.startAccel if starting else CS.aEgo
self.a_cruise = self.CP.startAccel if starting else a_ego
self.v_acc_start = CS.vEgo
self.a_acc_start = self.CP.startAccel if starting else CS.aEgo
self.a_acc_start = self.CP.startAccel if starting else a_ego
self.v_acc = CS.vEgo
self.a_acc = self.CP.startAccel if starting else CS.aEgo
self.a_acc = self.CP.startAccel if starting else a_ego
self.v_acc_sol = CS.vEgo
self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo
self.a_acc_sol = self.CP.startAccel if starting else a_ego
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
@ -351,15 +386,16 @@ class Planner(object):
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo,
self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat,
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:

96
selfdrive/controls/lib/radar_helpers.py
Unescape View File

@ -5,7 +5,9 @@ import platform
import numpy as np
from common.numpy_fast import clip, interp
from common.kalman.ekf import FastEKF1D, SimpleSensor
from common.kalman.simple_kalman import KF1D
NO_FUSION_SCORE = 100 # bad default fusion score
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
@ -23,13 +25,22 @@ v_stationary_thr = 4. # objects moving below this speed are classified as stat
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
_VLEAD_A = np.matrix([[1.0, ts], [0.0, 1.0]])
_VLEAD_C = np.matrix([1.0, 0.0])
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
_VLEAD_K = np.matrix([[ 0.1988689 ], [ 0.28555364]])
class Track(object):
def __init__(self):
self.ekf = None
self.stationary = True
self.initted = False
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned):
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
if self.initted:
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
@ -39,6 +50,7 @@ class Track(object):
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path
self.dPath = d_path
@ -47,59 +59,52 @@ class Track(object):
self.vLead = self.vRel + v_ego_t_aligned
if not self.initted:
self.initted = True
self.cnt = 1
self.vision_cnt = 0
self.vision = False
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.aLead = 0.
self.kf = KF1D(np.matrix([[self.vLead], [0.0]]), _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
# estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
# TODO: use Kalman filter
# neglect steer override cases as dPath is too noisy
v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
a_lead_unfilt = clip(a_lead_unfilt, -10., 10.)
self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
self.kf.update(self.vLead)
self.cnt += 1
self.vLeadK = float(self.kf.x[SPEED])
self.aLeadK = float(self.kf.x[ACCEL])
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
if self.ekf is None:
self.ekf = FastEKF1D(ts, 1e3, [0.1, 1])
self.ekf.state[SPEED] = self.vLead
self.ekf.state[ACCEL] = 0
self.lead_sensor = SimpleSensor(SPEED, 1, 2)
self.vLeadK = self.vLead
self.aLeadK = self.aLead
else:
self.ekf.update_scalar(self.lead_sensor.read(self.vLead))
self.ekf.predict(ts)
self.vLeadK = float(self.ekf.state[SPEED])
self.aLeadK = float(self.ekf.state[ACCEL])
if not self.initted:
self.cnt = 1
self.vision_cnt = 0
else:
self.cnt += 1
self.initted = True
self.vision = False
self.vision_score = NO_FUSION_SCORE
def mix_vision(self, dist_to_vision, rel_speed_diff):
def update_vision_score(self, dist_to_vision, rel_speed_diff):
# rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
# vision point is never stationary
self.vision_cnt += 1
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
self.vision_score = dist_to_vision + rel_speed_diff
else:
self.vision_score = NO_FUSION_SCORE
def update_vision_fusion(self):
# vision point is never stationary
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
@ -159,10 +164,6 @@ class Cluster(object):
def vLead(self):
return mean([t.vLead for t in self.tracks])
@property
def aLead(self):
return mean([t.aLead for t in self.tracks])
@property
def dPath(self):
return mean([t.dPath for t in self.tracks])
@ -183,6 +184,10 @@ class Cluster(object):
def vision(self):
return any([t.vision for t in self.tracks])
@property
def measured(self):
return any([t.measured for t in self.tracks])
@property
def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks])
@ -201,7 +206,6 @@ class Cluster(object):
lead.vRel = float(self.vRel)
lead.aRel = float(self.aRel)
lead.vLead = float(self.vLead)
lead.aLead = float(self.aLead)
lead.dPath = float(self.dPath)
lead.vLat = float(self.vLat)
lead.vLeadK = float(self.vLeadK)
@ -237,12 +241,14 @@ class Cluster(object):
# lat_corr used to be gated on enabled, now always running
t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
lat_corr = clip(t_lookahead * self.vLat, -1, 0)
d_path = max(d_path + lat_corr, 0)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact.
lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0.
# consider only cut-ins
d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
return d_path < 1.5 and not self.stationary and not self.oncoming
return abs(d_path) < 1.5 and not self.stationary and not self.oncoming
def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0:

6
selfdrive/controls/lib/speed_smoother.py
Unescape View File

@ -16,6 +16,12 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
dV = vT - vEgo
# recover quickly if dV is positive and aEgo is negative or viceversa
if dV > 0. and aEgo < 0.:
jMax *= 3.
elif dV < 0. and aEgo > 0.:
jMin *= 3.
tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
if (ts <= tDelta):

59
selfdrive/controls/lib/vehicle_model.py
Unescape View File

@ -10,36 +10,36 @@ from numpy.linalg import inv
# A depends on longitudinal speed, u, and vehicle parameters CP
def create_dyn_state_matrices(u, CP):
def create_dyn_state_matrices(u, VM):
A = np.zeros((2, 2))
B = np.zeros((2, 1))
A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u)
A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u
A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u)
A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u)
B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR
B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR
A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
return A, B
def kin_ss_sol(sa, u, CP):
def kin_ss_sol(sa, u, VM):
# kinematic solution, useful when speed ~ 0
K = np.zeros((2, 1))
K[0, 0] = CP.aR / CP.sR / CP.l * u
K[1, 0] = 1. / CP.sR / CP.l * u
K[0, 0] = VM.aR / VM.sR / VM.l * u
K[1, 0] = 1. / VM.sR / VM.l * u
return K * sa
def dyn_ss_sol(sa, u, CP):
def dyn_ss_sol(sa, u, VM):
# Dynamic solution, useful when speed > 0
A, B = create_dyn_state_matrices(u, CP)
A, B = create_dyn_state_matrices(u, VM)
return - np.matmul(inv(A), B) * sa
def calc_slip_factor(CP):
def calc_slip_factor(VM):
# the slip factor is a measure of how the curvature changes with speed
# it's positive for Oversteering vehicle, negative (usual case) otherwise
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
class VehicleModel(object):
@ -50,6 +50,16 @@ class VehicleModel(object):
self.update_state(init_state)
self.state_pred = np.zeros((self.steps, self.state.shape[0]))
self.CP = CP
# for math readability, convert long names car params into short names
self.m = CP.mass
self.j = CP.rotationalInertia
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
self.cF = CP.tireStiffnessFront
self.cR = CP.tireStiffnessRear
self.sR = CP.steerRatio
self.chi = CP.steerRatioRear
def update_state(self, state):
self.state = state
@ -58,33 +68,34 @@ class VehicleModel(object):
# if the speed is too small we can't use the dynamic model
# (tire slip is undefined), we then use the kinematic model
if u > 0.1:
return dyn_ss_sol(sa, u, self.CP)
return dyn_ss_sol(sa, u, self)
else:
return kin_ss_sol(sa, u, self.CP)
return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions
return self.curvature_factor(u) * sa / self.CP.sR
return self.curvature_factor(u) * sa / self.sR
def curvature_factor(self, u):
sf = calc_slip_factor(self.CP)
return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l
sf = calc_slip_factor(self)
return (1. - self.chi)/(1. - sf * u**2) / self.l
def get_steer_from_curvature(self, curv, u):
return curv * self.CP.sR * 1.0 / self.curvature_factor(u)
return curv * self.sR * 1.0 / self.curvature_factor(u)
def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self.CP)
A, B = create_dyn_state_matrices(u, self)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
if __name__ == '__main__':
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.honda.interface import CarInterface
# load car params
CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
#CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
print CP
VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15)
print calc_slip_factor(CP)
print calc_slip_factor(VM)

47
selfdrive/controls/radard.py
Unescape View File

@ -9,7 +9,8 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \
RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
@ -17,7 +18,6 @@ from common.params import Params
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.kalman.ekf import EKF, SimpleSensor
VISION_ONLY = False
DEBUG = False
#vision point
@ -96,11 +96,12 @@ def radard_thread(gctx=None):
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.aRel, None, False, None]
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = messaging.recv_sock(live100)
@ -130,7 +131,7 @@ def radard_thread(gctx=None):
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot())
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
@ -152,9 +153,7 @@ def radard_thread(gctx=None):
# *** compute the tracks ***
for ids in ar_pts:
# ignore the vision point for now
if ids == VISION_POINT and not VISION_ONLY:
continue
elif ids != VISION_POINT and VISION_ONLY:
if ids == VISION_POINT:
continue
rpt = ar_pts[ids]
@ -162,17 +161,30 @@ def radard_thread(gctx=None):
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks or rpt[5] == 1:
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned)
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2])
tracks[ids].mix_vision(dist_to_vision, rel_speed_diff)
# publish tracks (debugging)
dat = messaging.new_message()
@ -185,9 +197,12 @@ def radard_thread(gctx=None):
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f v: %4.1f d: %4.1f s: %1.0f" % \
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].stationary)
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)

7
selfdrive/manager.py
Unescape View File

@ -448,7 +448,8 @@ def manager_thread():
ignition = td is not None and td.health.started
ignition_seen = ignition_seen or ignition
should_start = ignition
params = Params()
should_start = ignition and (params.get("HasAcceptedTerms") == "1")
# start on gps in passive mode
if passive and not ignition_seen:
@ -460,7 +461,7 @@ def manager_thread():
if should_start:
if not started_ts:
Params().car_start()
params.car_start()
started_ts = sec_since_boot()
for p in car_started_processes:
if p == "loggerd" and logger_dead:
@ -583,6 +584,8 @@ def main():
params.put("IsRearViewMirror", "1")
if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")

2
selfdrive/radar/nidec/interface.py
Unescape View File

@ -33,6 +33,7 @@ class RadarInterface(object):
self.radar_fault = False
self.delay = 0.1 # Delay of radar
self.cutin_prediction = True
# Nidec
self.rcp = _create_nidec_can_parser()
@ -65,6 +66,7 @@ class RadarInterface(object):
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]

22
selfdrive/radar/toyota/interface.py
Unescape View File

@ -27,13 +27,14 @@ class RadarInterface(object):
def __init__(self):
# radar
self.pts = {}
self.ptsValid = {key: False for key in RADAR_MSGS}
self.validCnt = {key: 0 for key in RADAR_MSGS}
self.track_id = 0
self.delay = 0.0 # Delay of radar
# Nidec
self.rcp = _create_radard_can_parser()
self.cutin_prediction = False
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
@ -55,21 +56,21 @@ class RadarInterface(object):
errors.append("commIssue")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
#print "NEW TRACKS"
for ii in updated_messages:
cpt = self.rcp.vl[ii]
# a point needs one valid measurement before being considered
#if cpt['NEW_TRACK'] or cpt['LONG_DIST'] >= 255:
# self.ptsValid[ii] = False # reset validity
# TODO: find better way to eliminate both false positive and false negative
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
self.validCnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
self.ptsValid[ii] = True
self.validCnt[ii] += 1
else:
self.ptsValid[ii] = False
self.validCnt[ii] = max(self.validCnt[ii] -1, 0)
#print ii, self.validCnt[ii], cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
if self.ptsValid[ii]:
#print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
# radar point only valid if there have been enough valid measurements
if self.validCnt[ii] > 0:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
@ -79,6 +80,7 @@ class RadarInterface(object):
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = bool(cpt['VALID'])
else:
if ii in self.pts:
del self.pts[ii]

5
selfdrive/test/plant/maneuver.py
Unescape View File

@ -45,7 +45,7 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live100 = plant.step(speed_lead, current_button, grade)
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100= plant.step(speed_lead, current_button, grade)
if live100:
last_live100 = live100[-1]
@ -64,7 +64,8 @@ class Maneuver(object):
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
cruise_speed=last_live100.vCruise,
jerk_factor=last_live100.jerkFactor,
a_target=last_live100.aTarget)
a_target=last_live100.aTarget,
fcw=fcw)
print "maneuver end"

8
selfdrive/test/plant/maneuverplots.py
Unescape View File

@ -33,11 +33,13 @@ class ManeuverPlot(object):
self.v_target_array = []
self.fcw_array = []
self.title = title
def add_data(self, time, gas, brake, steer_torque, distance, speed,
acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead,
v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target):
v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
self.time_array.append(time)
self.gas_array.append(gas)
self.brake_array.append(brake)
@ -55,6 +57,7 @@ class ManeuverPlot(object):
self.cruise_speed_array.append(cruise_speed)
self.jerk_factor_array.append(jerk_factor)
self.a_target_array.append(a_target)
self.fcw_array.append(fcw)
def write_plot(self, path, maneuver_name):
@ -88,10 +91,11 @@ class ManeuverPlot(object):
plt.plot(
np.array(self.time_array), np.array(self.acceleration_array), 'g',
np.array(self.time_array), np.array(self.a_target_array), 'k--',
np.array(self.time_array), np.array(self.fcw_array), 'ro',
)
plt.xlabel('Time [s]')
plt.ylabel('Acceleration [m/s^2]')
plt.legend(['ego-plant', 'target'], loc=0)
plt.legend(['ego-plant', 'target', 'fcw'], loc=0)
plt.grid()
pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)

14
selfdrive/test/plant/plant.py
Unescape View File

@ -102,6 +102,7 @@ class Plant(object):
Plant.model = messaging.pub_sock(context, service_list['model'].port)
Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
Plant.plan = messaging.sub_sock(context, service_list['plan'].port)
Plant.messaging_initialized = True
self.angle_steer = 0.
@ -162,10 +163,14 @@ class Plant(object):
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live_msgs = []
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live_msgs.append(a.live100)
live100_msgs.append(a.live100)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.fcw:
fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
@ -264,6 +269,9 @@ class Plant(object):
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
@ -280,7 +288,7 @@ class Plant(object):
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live_msgs)
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100_msgs)
# simple engage in standalone mode
def plant_thread(rate=100):

43
selfdrive/test/tests/plant/test_longitudinal.py
Unescape View File

@ -197,9 +197,51 @@ maneuvers = [
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
duration=15.,
initial_speed=30.,
lead_relevancy=True,
initial_distance_lead=100.,
speed_lead_values=[20.],
speed_lead_breakpoints=[1.],
cruise_button_presses = []
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
duration=18.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 23.],
cruise_button_presses = []
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
duration=13.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 9.6],
cruise_button_presses = []
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
duration=8.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 7.],
cruise_button_presses = []
)
]
#maneuvers = [maneuvers[-1]]
def setup_output():
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
if not os.path.exists(os.path.join(output_dir, "index.html")):
@ -235,6 +277,7 @@ class LongitudinalControl(unittest.TestCase):
shutil.rmtree('/data/params', ignore_errors=True)
params = Params()
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
params.put("IsFcwEnabled", "1")
manager.gctx = {}
manager.prepare_managed_process('radard')

20
selfdrive/ui/ui.c
Unescape View File

@ -313,7 +313,7 @@ static bool try_load_intrinsics(mat3 *intrinsic_matrix) {
}
int i = 0;
JsonNode* json_num;
JsonNode* json_num;
json_foreach(json_num, intrinsic_json) {
intrinsic_matrix->v[i++] = json_num->number_;
}
@ -676,7 +676,7 @@ static void draw_frame(UIState *s) {
* Draw a rect at specific position with specific dimensions
*/
static void ui_draw_rounded_rect(
NVGcontext* c,
NVGcontext* c,
int x,
int y,
int width,
@ -725,7 +725,7 @@ static void ui_draw_world(UIState *s) {
if (!s->passive) {
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255));
draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255));
draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 49, nvgRGBA(255, 0, 0, 255));
}
}
@ -811,7 +811,7 @@ static void ui_draw_vision(UIState *s) {
snprintf(speed_str, sizeof(speed_str), "%3d KPH",
(int)(scene->v_cruise + 0.5));
} else {
/* Convert KPH to MPH. Using an approximated mph to kph
/* Convert KPH to MPH. Using an approximated mph to kph
conversion factor of 1.609 because this is what the Honda
hud seems to be using */
snprintf(speed_str, sizeof(speed_str), "%3d MPH",
@ -896,7 +896,7 @@ static void ui_draw_alerts(UIState *s) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClear(GL_STENCIL_BUFFER_BIT);
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
// draw alert text
@ -1032,7 +1032,7 @@ static void ui_update(UIState *s) {
polls[3].events = ZMQ_POLLIN;
polls[4].socket = s->livempc_sock_raw;
polls[4].events = ZMQ_POLLIN;
int num_polls = 5;
if (s->vision_connected) {
assert(s->ipc_fd >= 0);
@ -1192,17 +1192,17 @@ static void ui_update(UIState *s) {
} else if (eventd.which == cereal_Event_liveMpc) {
struct cereal_LiveMpcData datad;
cereal_read_LiveMpcData(&datad, eventd.liveMpc);
capn_list32 x_list = datad.x;
capn_resolve(&x_list.p);
for (int i = 0; i < 50; i++){
s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i));
}
capn_list32 y_list = datad.y;
capn_resolve(&y_list.p);
for (int i = 0; i < 50; i++){
s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i));
}
@ -1310,7 +1310,7 @@ int main() {
while (!do_exit) {
pthread_mutex_lock(&s->lock);
ui_update(s);
if (s->awake) {
ui_draw(s);

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.
Write Preview
Loading…
Cancel
Save