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
Unified 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!) - Toyota RAV-4 2016+ with TSS-P (alpha!)
- By default it uses stock Toyota ACC for longitudinal control - 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!) - Toyota Prius 2017 (alpha!)
- By default it uses stock Toyota ACC for longitudinal control - 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 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 # these are optional and valid if they are not NaN
aRel @4 :Float32; # m/s^2 aRel @4 :Float32; # m/s^2
yvRel @5 :Float32; # m/s yvRel @5 :Float32; # m/s
# some radars flag measurements VS estimates
measured @6 :Bool;
} }
} }
@ -252,22 +255,22 @@ struct CarParams {
enableGas @4 :Bool; enableGas @4 :Bool;
enableBrake @5 :Bool; enableBrake @5 :Bool;
enableCruise @6 :Bool; enableCruise @6 :Bool;
enableCamera @27 :Bool; enableCamera @26 :Bool;
enableDsu @28 :Bool; # driving support unit enableDsu @27 :Bool; # driving support unit
enableApgs @29 :Bool; # advanced parking guidance system enableApgs @28 :Bool; # advanced parking guidance system
minEnableSpeed @18 :Float32; minEnableSpeed @17 :Float32;
safetyModel @19 :Int16; safetyModel @18 :Int16;
steerMaxBP @20 :List(Float32); steerMaxBP @19 :List(Float32);
steerMaxV @21 :List(Float32); steerMaxV @20 :List(Float32);
gasMaxBP @22 :List(Float32); gasMaxBP @21 :List(Float32);
gasMaxV @23 :List(Float32); gasMaxV @22 :List(Float32);
brakeMaxBP @24 :List(Float32); brakeMaxBP @23 :List(Float32);
brakeMaxV @25 :List(Float32); brakeMaxV @24 :List(Float32);
longPidDeadzoneBP @33 :List(Float32); longPidDeadzoneBP @32 :List(Float32);
longPidDeadzoneV @34 :List(Float32); longPidDeadzoneV @33 :List(Float32);
enum SafetyModels { enum SafetyModels {
# does NOT match board setting # does NOT match board setting
@ -278,33 +281,32 @@ struct CarParams {
} }
# things about the car in the manual # things about the car in the manual
m @7 :Float32; # [kg] running weight mass @7 :Float32; # [kg] running weight
l @8 :Float32; # [m] wheelbase wheelbase @8 :Float32; # [m] distance from rear to front axle
sR @9 :Float32; # [] steering ratio centerToFront @9 :Float32; # [m] GC distance to front axle
aF @10 :Float32; # [m] GC distance to front axle steerRatio @10 :Float32; # [] ratio between front wheels and steering wheel angles
aR @11 :Float32; # [m] GC distance to rear axle steerRatioRear @11 :Float32; # [] rear steering ratio wrt front steering (usually 0)
chi @12 :Float32; # [] rear steering ratio wrt front steering (usually 0)
# things we can derive # things we can derive
j @13 :Float32; # [kg*m2] body rotational inertia rotationalInertia @12 :Float32; # [kg*m2] body rotational inertia
cF @14 :Float32; # [N/rad] front tire coeff of stiff tireStiffnessFront @13 :Float32; # [N/rad] front tire coeff of stiff
cR @15 :Float32; # [N/rad] rear tire coeff of stiff tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff
# Kp and Ki for the lateral control # Kp and Ki for the lateral control
steerKp @16 :Float32; steerKp @15 :Float32;
steerKi @17 :Float32; steerKi @16 :Float32;
steerKf @26 :Float32; steerKf @25 :Float32;
# Kp and Ki for the longitudinal control # Kp and Ki for the longitudinal control
longitudinalKpBP @37 :List(Float32); longitudinalKpBP @36 :List(Float32);
longitudinalKpV @38 :List(Float32); longitudinalKpV @37 :List(Float32);
longitudinalKiBP @39 :List(Float32); longitudinalKiBP @38 :List(Float32);
longitudinalKiV @40 :List(Float32); longitudinalKiV @39 :List(Float32);
steerLimitAlert @30 :Bool; steerLimitAlert @29 :Bool;
vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake directAccelControl @31 :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 stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
startAccel @36 :Float32; # Required acceleraton to overcome creep braking 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; vRel @2 :Float32;
aRel @3 :Float32; aRel @3 :Float32;
vLead @4 :Float32; vLead @4 :Float32;
aLead @5 :Float32; aLeadDEPRECATED @5 :Float32;
dPath @6 :Float32; dPath @6 :Float32;
vLat @7 :Float32; vLat @7 :Float32;
vLeadK @8 :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. # 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. # Access and set the state of the filter directly with ekf.state and ekf.covar.
class SensorReading: class SensorReading:
# Given a perfect model and no noise, data = obs_model * state # Given a perfect model and no noise, data = obs_model * state
def __init__(self, data, covar, obs_model): def __init__(self, data, covar, obs_model):
@ -33,7 +34,7 @@ class SensorReading:
# A generic sensor class that does no pre-processing of data # A generic sensor class that does no pre-processing of data
class SimpleSensor: class SimpleSensor:
# obs_model can be # obs_model can be
# a full obesrvation model matrix, or # a full obesrvation model matrix, or
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be # covar can be
@ -131,11 +132,11 @@ class EKF:
# like update but knowing that measurment is a scalar # like update but knowing that measurment is a scalar
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state) # 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 # 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 # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
# self.state += np.matmul(kalman_gain, innovation) # self.state += np.matmul(kalman_gain, innovation)
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model) # 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)) # 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 # written without np.matmul
@ -174,7 +175,7 @@ class EKF:
#! Clip covariance to avoid explosions #! Clip covariance to avoid explosions
self.covar = np.clip(self.covar,-1e10,1e10) self.covar = np.clip(self.covar,-1e10,1e10)
@abc.abstractmethod @abc.abstractmethod
def calc_transfer_fun(self, dt): def calc_transfer_fun(self, dt):
"""Return a tuple with the transfer function and transfer function jacobian """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 and using it during calcualtion of A and J
""" """
class FastEKF1D(EKF): class FastEKF1D(EKF):
"""Fast version of EKF for 1D problems with scalar readings.""" """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, "IsMetric": TxType.PERSISTANT,
"IsRearViewMirror": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT,
"IsFcwEnabled": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT,
"HasAcceptedTerms": TxType.PERSISTANT,
# written: visiond # written: visiond
# read: visiond, controlsd # read: visiond, controlsd
"CalibrationParams": TxType.PERSISTANT, "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 . import hondacan
from .values import AH 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): def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams # hyst params... TODO: move these to VehicleParams
@ -126,7 +122,7 @@ class CarController(object):
GAS_MAX = 1004 GAS_MAX = 1004
BRAKE_MAX = 1024/4 BRAKE_MAX = 1024/4
if CS.civic: 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 STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv: elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value 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 from selfdrive.services import service_list
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from selfdrive.car.honda.carstate import CarState, get_can_parser 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.car.honda.values import CruiseButtons, CM, BP, AH
from selfdrive.controls.lib.planner import A_ACC_MAX from selfdrive.controls.lib.planner import A_ACC_MAX
@ -20,6 +19,11 @@ except ImportError:
CarController = None 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): def compute_gb_honda(accel, speed):
creep_brake = 0.0 creep_brake = 0.0
creep_speed = 2.3 creep_speed = 2.3
@ -108,7 +112,7 @@ class CarInterface(object):
def calc_accel_override(a_ego, a_target, v_ego, v_target): def calc_accel_override(a_ego, a_target, v_ego, v_target):
eA = a_ego - a_target eA = a_ego - a_target
valuesA = [1.0, 0.1] valuesA = [1.0, 0.1]
bpA = [0.0, 0.5] bpA = [0.3, 1.1]
eV = v_ego - v_target eV = v_ego - v_target
valuesV = [1.0, 0.1] 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 # FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars # scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo mass_civic = 2923./2.205 + std_cargo
l_civic = 2.70 wheelbase_civic = 2.70
aF_civic = l_civic * 0.4 centerToFront_civic = wheelbase_civic * 0.4
aR_civic = l_civic - aF_civic centerToRear_civic = wheelbase_civic - centerToFront_civic
j_civic = 2500 rotationalInertia_civic = 2500
cF_civic = 85400 tireStiffnessFront_civic = 85400
cR_civic = 90000 tireStiffnessRear_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING": if candidate == "HONDA CIVIC 2016 TOURING":
stop_and_go = True stop_and_go = True
ret.m = m_civic ret.mass = mass_civic
ret.l = l_civic ret.wheelbase = wheelbase_civic
ret.aF = aF_civic ret.centerToFront = centerToFront_civic
ret.sR = 13.0 ret.steerRatio = 13.0
# Civic at comma has modified steering FW, so different tuning for the Neo in that car # 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.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpBP = [0., 5., 35.]
@ -168,10 +172,10 @@ class CarInterface(object):
ret.longitudinalKiV = [0.54, 0.36] ret.longitudinalKiV = [0.54, 0.36]
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS": elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
stop_and_go = False stop_and_go = False
ret.m = 3095./2.205 + std_cargo ret.mass = 3095./2.205 + std_cargo
ret.l = 2.67 ret.wheelbase = 2.67
ret.aF = ret.l * 0.37 ret.centerToFront = ret.wheelbase * 0.37
ret.sR = 15.3 ret.steerRatio = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car # 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'] 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] 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] ret.longitudinalKiV = [0.18, 0.12]
elif candidate == "HONDA ACCORD 2016 TOURING": elif candidate == "HONDA ACCORD 2016 TOURING":
stop_and_go = False stop_and_go = False
ret.m = 3580./2.205 + std_cargo ret.mass = 3580./2.205 + std_cargo
ret.l = 2.74 ret.wheelbase = 2.74
ret.aF = ret.l * 0.38 ret.centerToFront = ret.wheelbase * 0.38
ret.sR = 15.3 ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24 ret.steerKp, ret.steerKi = 0.8, 0.24
ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpBP = [0., 5., 35.]
@ -194,10 +198,10 @@ class CarInterface(object):
ret.longitudinalKiV = [0.18, 0.12] ret.longitudinalKiV = [0.18, 0.12]
elif candidate == "HONDA CR-V 2016 TOURING": elif candidate == "HONDA CR-V 2016 TOURING":
stop_and_go = False stop_and_go = False
ret.m = 3572./2.205 + std_cargo ret.mass = 3572./2.205 + std_cargo
ret.l = 2.62 ret.wheelbase = 2.62
ret.aF = ret.l * 0.41 ret.centerToFront = ret.wheelbase * 0.41
ret.sR = 15.3 ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24 ret.steerKp, ret.steerKi = 0.8, 0.24
ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpBP = [0., 5., 35.]
@ -214,18 +218,23 @@ class CarInterface(object):
# conflict with PCM acc # conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS 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 # TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase # 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 # 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 # 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.tireStiffnessFront = tireStiffnessFront_civic * \
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_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 # no rear steering, at least on the listed cars above
ret.chi = 0. ret.steerRatioRear = 0.
# no max steer limit VS speed # no max steer limit VS speed
ret.steerMaxBP = [0.] # m/s 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 # FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars # scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo mass_civic = 2923./2.205 + std_cargo
l_civic = 2.70 wheelbase_civic = 2.70
aF_civic = l_civic * 0.4 centerToFront_civic = wheelbase_civic * 0.4
aR_civic = l_civic - aF_civic centerToRear_civic = wheelbase_civic - centerToFront_civic
j_civic = 2500 rotationalInertia_civic = 2500
cF_civic = 85400 tireStiffnessFront_civic = 85400
cR_civic = 90000 tireStiffnessRear_civic = 90000
stop_and_go = True stop_and_go = True
ret.m = 3045./2.205 + std_cargo ret.mass = 3045./2.205 + std_cargo
ret.l = 2.70 ret.wheelbase = 2.70
ret.aF = ret.l * 0.44 ret.centerToFront = ret.wheelbase * 0.44
ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
ret.steerKp, ret.steerKi = 0.6, 0.05 ret.steerKp, ret.steerKi = 0.6, 0.05
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 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 elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go
ret.minEnableSpeed = 19. * CV.MPH_TO_MS 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 # TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase # 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 # 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 # 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.tireStiffnessFront = tireStiffnessFront_civic * \
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_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 # no rear steering, at least on the listed cars above
ret.chi = 0. ret.steerRatioRear = 0.
# steer, gas, brake limitations VS speed # steer, gas, brake limitations VS speed
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph 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) rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset # learned angle offset
angle_offset = 0. angle_offset = 1.5 # Default model bias
calibration_params = params.get("CalibrationParams") calibration_params = params.get("CalibrationParams")
if calibration_params: if calibration_params:
try: 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. min_learn_speed = 1.
# learn less at low speed or when turning # 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: # only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override: if lateral_control and not steer_override:

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

@ -1,11 +1,14 @@
import math import math
import numpy as np
from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py 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 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 # 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 = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz _DT_MPC = 0.05 # 20Hz
@ -24,6 +27,7 @@ def get_steer_max(CP, v_ego):
class LatControl(object): class LatControl(object):
def __init__(self, VM): def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) 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() self.setup_mpc()
def setup_mpc(self): def setup_mpc(self):
@ -61,7 +65,7 @@ class LatControl(object):
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay # 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 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, 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] delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired 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.angle_steers_des_time = cur_time
self.mpc_updated = True 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: if v_ego < 0.3 or not active:
output_steer = 0.0 output_steer = 0.0
self.pid.reset() 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(3,3) = 1.0;
Q(4,4) = 2.0; Q(4,4) = 1.0;
// Terminal cost // Terminal cost
Function hN; 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[1] = + tmpFx[4];
tmpQ2[2] = + tmpFx[8]; tmpQ2[2] = + tmpFx[8];
tmpQ2[3] = + tmpFx[12]; tmpQ2[3] = + tmpFx[12];
tmpQ2[4] = + tmpFx[16]*(real_t)2.0000000000000000e+00; tmpQ2[4] = + tmpFx[16];
tmpQ2[5] = + tmpFx[1]; tmpQ2[5] = + tmpFx[1];
tmpQ2[6] = + tmpFx[5]; tmpQ2[6] = + tmpFx[5];
tmpQ2[7] = + tmpFx[9]; tmpQ2[7] = + tmpFx[9];
tmpQ2[8] = + tmpFx[13]; tmpQ2[8] = + tmpFx[13];
tmpQ2[9] = + tmpFx[17]*(real_t)2.0000000000000000e+00; tmpQ2[9] = + tmpFx[17];
tmpQ2[10] = + tmpFx[2]; tmpQ2[10] = + tmpFx[2];
tmpQ2[11] = + tmpFx[6]; tmpQ2[11] = + tmpFx[6];
tmpQ2[12] = + tmpFx[10]; tmpQ2[12] = + tmpFx[10];
tmpQ2[13] = + tmpFx[14]; tmpQ2[13] = + tmpFx[14];
tmpQ2[14] = + tmpFx[18]*(real_t)2.0000000000000000e+00; tmpQ2[14] = + tmpFx[18];
tmpQ2[15] = + tmpFx[3]; tmpQ2[15] = + tmpFx[3];
tmpQ2[16] = + tmpFx[7]; tmpQ2[16] = + tmpFx[7];
tmpQ2[17] = + tmpFx[11]; tmpQ2[17] = + tmpFx[11];
tmpQ2[18] = + tmpFx[15]; 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[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[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]; 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[1] = + tmpFu[1];
tmpR2[2] = + tmpFu[2]; tmpR2[2] = + tmpFu[2];
tmpR2[3] = + tmpFu[3]; 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]; 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[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1];
tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2]; tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2];
tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3]; 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]; 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 numpy as np
import math import math
from collections import defaultdict
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from common.params import Params 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_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.] _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 # max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) 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 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_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_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
a_target[1] = min(a_target[1], a_x_allowed) 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): class FCWChecker(object):
def __init__(self): def __init__(self):
self.fcw_count = 0 self.reset_lead(0.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
def reset_lead(self, cur_time): def reset_lead(self, cur_time):
self.last_fcw_a = 0.0
self.v_lead_max = 0.0 self.v_lead_max = 0.0
self.lead_seen_t = cur_time 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): @staticmethod
min_a_mpc = min(list(mpc_solution[0].a_ego)[1:]) 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) self.v_lead_max = max(self.v_lead_max, v_lead)
if (fcw_lead > 0.99 if (fcw_lead > 0.99):
and v_ego > 5.0 ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
and min_a_mpc < -4.0 self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
and self.v_lead_max > 2.5 self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
and v_ego > v_lead self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
and self.lead_seen_t < cur_time - 2.0 self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
and abs(y_lead) < 1.0 self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
and abs(vlat_lead) < 0.3 self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
and not blinkers): self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
self.fcw_count += 1 self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time:
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_time = cur_time
self.last_fcw_a = min_a_mpc self.last_fcw_a = self.last_min_a
return True return True
else:
self.fcw_count = 0
return False return False
@ -214,6 +246,8 @@ class LongitudinalMpc(object):
self.libmpc.init() self.libmpc.init()
self.cur_state[0].v_ego = CS.vEgo self.cur_state[0].v_ego = CS.vEgo
self.cur_state[0].a_ego = 0.0 self.cur_state[0].a_ego = 0.0
self.v_mpc = CS.vEgo
self.a_mpc = CS.aEgo
self.prev_lead_status = False self.prev_lead_status = False
@ -257,32 +291,33 @@ class Planner(object):
self.fcw_checker = FCWChecker() self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled self.fcw_enabled = fcw_enabled
def choose_solution(self, v_cruise_setpoint): def choose_solution(self, v_cruise_setpoint, enabled):
solutions = {'cruise': self.v_cruise} if enabled:
if self.mpc1.prev_lead_status: solutions = {'cruise': self.v_cruise}
solutions['mpc1'] = self.mpc1.v_mpc if self.mpc1.prev_lead_status:
if self.mpc2.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc
solutions['mpc2'] = self.mpc2.v_mpc if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
slowest = min(solutions, key=solutions.get)
if _DEBUG:
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol if _DEBUG:
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise 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
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1': # Choose lowest of MPC and cruise
self.v_acc = self.mpc1.v_mpc if slowest == 'mpc1':
self.a_acc = self.mpc1.a_mpc self.v_acc = self.mpc1.v_mpc
elif slowest == 'mpc2': self.a_acc = self.mpc1.a_mpc
self.v_acc = self.mpc2.v_mpc elif slowest == 'mpc2':
self.a_acc = self.mpc2.a_mpc self.v_acc = self.mpc2.v_mpc
elif slowest == 'cruise': self.a_acc = self.mpc2.a_mpc
self.v_acc = self.v_cruise elif slowest == 'cruise':
self.a_acc = self.a_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]) 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, self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint, v_cruise_setpoint,
accel_limits[1], accel_limits[0], accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[1], jerk_limits[0],
jerk_limits[0],
_DT_MPC) _DT_MPC)
else: else:
starting = LoC.long_control_state == LongCtrlState.starting starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
self.v_cruise = CS.vEgo 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.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.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.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.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) 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.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, 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 # determine fcw
if self.mpc1.new_lead: if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time) self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat, 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) \ self.lead_1.fcw, blinkers) \
and not CS.brakePressed and not CS.brakePressed
if self.fcw: if self.fcw:

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

@ -5,7 +5,9 @@ import platform
import numpy as np import numpy as np
from common.numpy_fast import clip, interp 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 # radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum 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_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 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): class Track(object):
def __init__(self): def __init__(self):
self.ekf = None self.ekf = None
self.stationary = True self.stationary = True
self.initted = False 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: if self.initted:
self.dPathPrev = self.dPath self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead self.vLeadPrev = self.vLead
@ -39,6 +50,7 @@ class Track(object):
self.dRel = d_rel # LONG_DIST self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path # compute distance to path
self.dPath = d_path self.dPath = d_path
@ -47,59 +59,52 @@ class Track(object):
self.vLead = self.vRel + v_ego_t_aligned self.vLead = self.vRel + v_ego_t_aligned
if not self.initted: 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.aRel = 0. # nidec gives no information about this
self.vLat = 0. self.vLat = 0.
self.aLead = 0. self.kf = KF1D(np.matrix([[self.vLead], [0.0]]), _VLEAD_A, _VLEAD_C, _VLEAD_K)
else: else:
# estimate acceleration # estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.) a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel 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 self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts self.kf.update(self.vLead)
a_lead_unfilt = clip(a_lead_unfilt, -10., 10.)
self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead self.cnt += 1
self.vLeadK = float(self.kf.x[SPEED])
self.aLeadK = float(self.kf.x[ACCEL])
if self.stationary: if self.stationary:
# stationary objects can become non stationary, but not the other way around # 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.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr self.oncoming = self.vLead < v_oncoming_thr
if self.ekf is None: self.vision_score = NO_FUSION_SCORE
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
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 # rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.: if dist_to_vision < 4.0 and rel_speed_diff < 10.:
# vision point is never stationary self.vision_score = dist_to_vision + rel_speed_diff
self.vision_cnt += 1 else:
# don't trust 1 or 2 fusions until model quality is much better self.vision_score = NO_FUSION_SCORE
if self.vision_cnt >= 3:
self.vision = True def update_vision_fusion(self):
self.stationary = False # 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): def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension # Weigh y higher since radar is inaccurate in this dimension
@ -159,10 +164,6 @@ class Cluster(object):
def vLead(self): def vLead(self):
return mean([t.vLead for t in self.tracks]) return mean([t.vLead for t in self.tracks])
@property
def aLead(self):
return mean([t.aLead for t in self.tracks])
@property @property
def dPath(self): def dPath(self):
return mean([t.dPath for t in self.tracks]) return mean([t.dPath for t in self.tracks])
@ -183,6 +184,10 @@ class Cluster(object):
def vision(self): def vision(self):
return any([t.vision for t in self.tracks]) return any([t.vision for t in self.tracks])
@property
def measured(self):
return any([t.measured for t in self.tracks])
@property @property
def vision_cnt(self): def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks]) return max([t.vision_cnt for t in self.tracks])
@ -201,7 +206,6 @@ class Cluster(object):
lead.vRel = float(self.vRel) lead.vRel = float(self.vRel)
lead.aRel = float(self.aRel) lead.aRel = float(self.aRel)
lead.vLead = float(self.vLead) lead.vLead = float(self.vLead)
lead.aLead = float(self.aLead)
lead.dPath = float(self.dPath) lead.dPath = float(self.dPath)
lead.vLat = float(self.vLat) lead.vLat = float(self.vLat)
lead.vLeadK = float(self.vLeadK) lead.vLeadK = float(self.vLeadK)
@ -237,12 +241,14 @@ class Cluster(object):
# lat_corr used to be gated on enabled, now always running # lat_corr used to be gated on enabled, now always running
t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v) 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): def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0: 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 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) tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
if (ts <= tDelta): 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 # 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)) A = np.zeros((2, 2))
B = np.zeros((2, 1)) B = np.zeros((2, 1))
A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u) A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u) A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u) A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
return A, B return A, B
def kin_ss_sol(sa, u, CP): def kin_ss_sol(sa, u, VM):
# kinematic solution, useful when speed ~ 0 # kinematic solution, useful when speed ~ 0
K = np.zeros((2, 1)) K = np.zeros((2, 1))
K[0, 0] = CP.aR / CP.sR / CP.l * u K[0, 0] = VM.aR / VM.sR / VM.l * u
K[1, 0] = 1. / CP.sR / CP.l * u K[1, 0] = 1. / VM.sR / VM.l * u
return K * sa return K * sa
def dyn_ss_sol(sa, u, CP): def dyn_ss_sol(sa, u, VM):
# Dynamic solution, useful when speed > 0 # 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 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 # the slip factor is a measure of how the curvature changes with speed
# it's positive for Oversteering vehicle, negative (usual case) otherwise # 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): class VehicleModel(object):
@ -50,6 +50,16 @@ class VehicleModel(object):
self.update_state(init_state) self.update_state(init_state)
self.state_pred = np.zeros((self.steps, self.state.shape[0])) self.state_pred = np.zeros((self.steps, self.state.shape[0]))
self.CP = CP 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): def update_state(self, state):
self.state = state self.state = state
@ -58,33 +68,34 @@ class VehicleModel(object):
# if the speed is too small we can't use the dynamic model # if the speed is too small we can't use the dynamic model
# (tire slip is undefined), we then use the kinematic model # (tire slip is undefined), we then use the kinematic model
if u > 0.1: if u > 0.1:
return dyn_ss_sol(sa, u, self.CP) return dyn_ss_sol(sa, u, self)
else: else:
return kin_ss_sol(sa, u, self.CP) return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa, u): def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions # 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): def curvature_factor(self, u):
sf = calc_slip_factor(self.CP) sf = calc_slip_factor(self)
return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l return (1. - self.chi)/(1. - sf * u**2) / self.l
def get_steer_from_curvature(self, curv, u): 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): def state_prediction(self, sa, u):
# U is the matrix of the controls # U is the matrix of the controls
# u is the long speed # 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 return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
if __name__ == '__main__': if __name__ == '__main__':
from selfdrive.car.toyota.interface import CarInterface from selfdrive.car.honda.interface import CarInterface
# load car params # 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 print CP
VM = VehicleModel(CP) VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15) 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.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner 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.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from cereal import car 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.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.kalman.ekf import EKF, SimpleSensor from common.kalman.ekf import EKF, SimpleSensor
VISION_ONLY = False
DEBUG = False DEBUG = False
#vision point #vision point
@ -96,11 +96,12 @@ def radard_thread(gctx=None):
rk = Ratekeeper(rate, print_delay_threshold=np.inf) rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1: while 1:
rr = RI.update() rr = RI.update()
ar_pts = {} ar_pts = {}
for pt in rr.points: 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 # receive the live100s
l100 = messaging.recv_sock(live100) 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.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv) ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), 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: else:
ekfv.state[XV] = PP.lead_dist ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
@ -152,9 +153,7 @@ def radard_thread(gctx=None):
# *** compute the tracks *** # *** compute the tracks ***
for ids in ar_pts: for ids in ar_pts:
# ignore the vision point for now # ignore the vision point for now
if ids == VISION_POINT and not VISION_ONLY: if ids == VISION_POINT:
continue
elif ids != VISION_POINT and VISION_ONLY:
continue continue
rpt = ar_pts[ids] rpt = ar_pts[ids]
@ -162,17 +161,30 @@ def radard_thread(gctx=None):
cur_time = float(rk.frame)/rate cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0]) 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)) 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 # 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] = 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) # publish tracks (debugging)
dat = messaging.new_message() dat = messaging.new_message()
@ -185,9 +197,12 @@ def radard_thread(gctx=None):
for cnt, ids in enumerate(tracks.keys()): for cnt, ids in enumerate(tracks.keys()):
if DEBUG: 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, (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].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) 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 = td is not None and td.health.started
ignition_seen = ignition_seen or ignition 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 # start on gps in passive mode
if passive and not ignition_seen: if passive and not ignition_seen:
@ -460,7 +461,7 @@ def manager_thread():
if should_start: if should_start:
if not started_ts: if not started_ts:
Params().car_start() params.car_start()
started_ts = sec_since_boot() started_ts = sec_since_boot()
for p in car_started_processes: for p in car_started_processes:
if p == "loggerd" and logger_dead: if p == "loggerd" and logger_dead:
@ -583,6 +584,8 @@ def main():
params.put("IsRearViewMirror", "1") params.put("IsRearViewMirror", "1")
if params.get("IsFcwEnabled") is None: if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1") params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
params.put("Passive", "1" if os.getenv("PASSIVE") else "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.radar_fault = False
self.delay = 0.1 # Delay of radar self.delay = 0.1 # Delay of radar
self.cutin_prediction = True
# Nidec # Nidec
self.rcp = _create_nidec_can_parser() self.rcp = _create_nidec_can_parser()
@ -65,6 +66,7 @@ class RadarInterface(object):
self.pts[ii].vRel = cpt['REL_SPEED'] self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan') self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan') self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else: else:
if ii in self.pts: if ii in self.pts:
del self.pts[ii] del self.pts[ii]

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

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

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

@ -33,11 +33,13 @@ class ManeuverPlot(object):
self.v_target_array = [] self.v_target_array = []
self.fcw_array = []
self.title = title self.title = title
def add_data(self, time, gas, brake, steer_torque, distance, speed, def add_data(self, time, gas, brake, steer_torque, distance, speed,
acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, 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.time_array.append(time)
self.gas_array.append(gas) self.gas_array.append(gas)
self.brake_array.append(brake) self.brake_array.append(brake)
@ -55,6 +57,7 @@ class ManeuverPlot(object):
self.cruise_speed_array.append(cruise_speed) self.cruise_speed_array.append(cruise_speed)
self.jerk_factor_array.append(jerk_factor) self.jerk_factor_array.append(jerk_factor)
self.a_target_array.append(a_target) self.a_target_array.append(a_target)
self.fcw_array.append(fcw)
def write_plot(self, path, maneuver_name): def write_plot(self, path, maneuver_name):
@ -88,10 +91,11 @@ class ManeuverPlot(object):
plt.plot( plt.plot(
np.array(self.time_array), np.array(self.acceleration_array), 'g', 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.a_target_array), 'k--',
np.array(self.time_array), np.array(self.fcw_array), 'ro',
) )
plt.xlabel('Time [s]') plt.xlabel('Time [s]')
plt.ylabel('Acceleration [m/s^2]') plt.ylabel('Acceleration [m/s^2]')
plt.legend(['ego-plant', 'target'], loc=0) plt.legend(['ego-plant', 'target', 'fcw'], loc=0)
plt.grid() plt.grid()
pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000) 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.model = messaging.pub_sock(context, service_list['model'].port)
Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port) Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
Plant.live100 = messaging.sub_sock(context, service_list['live100'].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 Plant.messaging_initialized = True
self.angle_steer = 0. self.angle_steer = 0.
@ -162,10 +163,14 @@ class Plant(object):
self.cp.update_can(can_msgs) self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting *** # ******** get live100 messages for plotting ***
live_msgs = [] live100_msgs = []
for a in messaging.drain_sock(Plant.live100): 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']: if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
@ -264,6 +269,9 @@ class Plant(object):
x.points = [0.0]*50 x.points = [0.0]*50
x.prob = 1.0 x.prob = 1.0
x.std = 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.calStatus = 1
cal.liveCalibration.calPerc = 100 cal.liveCalibration.calPerc = 100
# fake values? # fake values?
@ -280,7 +288,7 @@ class Plant(object):
self.distance_lead_prev = distance_lead self.distance_lead_prev = distance_lead
self.rk.keep_time() 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 # simple engage in standalone mode
def plant_thread(rate=100): 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, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1), (CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)] (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(): def setup_output():
output_dir = os.path.join(os.getcwd(), 'out/longitudinal') output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
if not os.path.exists(os.path.join(output_dir, "index.html")): 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) shutil.rmtree('/data/params', ignore_errors=True)
params = Params() params = Params()
params.put("Passive", "1" if os.getenv("PASSIVE") else "0") params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
params.put("IsFcwEnabled", "1")
manager.gctx = {} manager.gctx = {}
manager.prepare_managed_process('radard') 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; int i = 0;
JsonNode* json_num; JsonNode* json_num;
json_foreach(json_num, intrinsic_json) { json_foreach(json_num, intrinsic_json) {
intrinsic_matrix->v[i++] = json_num->number_; 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 * Draw a rect at specific position with specific dimensions
*/ */
static void ui_draw_rounded_rect( static void ui_draw_rounded_rect(
NVGcontext* c, NVGcontext* c,
int x, int x,
int y, int y,
int width, int width,
@ -725,7 +725,7 @@ static void ui_draw_world(UIState *s) {
if (!s->passive) { if (!s->passive) {
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255)); 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", snprintf(speed_str, sizeof(speed_str), "%3d KPH",
(int)(scene->v_cruise + 0.5)); (int)(scene->v_cruise + 0.5));
} else { } 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 conversion factor of 1.609 because this is what the Honda
hud seems to be using */ hud seems to be using */
snprintf(speed_str, sizeof(speed_str), "%3d MPH", 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); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClear(GL_STENCIL_BUFFER_BIT); glClear(GL_STENCIL_BUFFER_BIT);
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
// draw alert text // draw alert text
@ -1032,7 +1032,7 @@ static void ui_update(UIState *s) {
polls[3].events = ZMQ_POLLIN; polls[3].events = ZMQ_POLLIN;
polls[4].socket = s->livempc_sock_raw; polls[4].socket = s->livempc_sock_raw;
polls[4].events = ZMQ_POLLIN; polls[4].events = ZMQ_POLLIN;
int num_polls = 5; int num_polls = 5;
if (s->vision_connected) { if (s->vision_connected) {
assert(s->ipc_fd >= 0); assert(s->ipc_fd >= 0);
@ -1192,17 +1192,17 @@ static void ui_update(UIState *s) {
} else if (eventd.which == cereal_Event_liveMpc) { } else if (eventd.which == cereal_Event_liveMpc) {
struct cereal_LiveMpcData datad; struct cereal_LiveMpcData datad;
cereal_read_LiveMpcData(&datad, eventd.liveMpc); cereal_read_LiveMpcData(&datad, eventd.liveMpc);
capn_list32 x_list = datad.x; capn_list32 x_list = datad.x;
capn_resolve(&x_list.p); capn_resolve(&x_list.p);
for (int i = 0; i < 50; i++){ for (int i = 0; i < 50; i++){
s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i)); s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i));
} }
capn_list32 y_list = datad.y; capn_list32 y_list = datad.y;
capn_resolve(&y_list.p); capn_resolve(&y_list.p);
for (int i = 0; i < 50; i++){ for (int i = 0; i < 50; i++){
s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i));
} }
@ -1310,7 +1310,7 @@ int main() {
while (!do_exit) { while (!do_exit) {
pthread_mutex_lock(&s->lock); pthread_mutex_lock(&s->lock);
ui_update(s); ui_update(s);
if (s->awake) { if (s->awake) {
ui_draw(s); ui_draw(s);

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.
Write Preview
Loading…
Cancel
Save