mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 10:03:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.9 tweaks

Browse Source 
old-commit-hash: 1ad9cc8c67
This commit is contained in:
Vehicle Researcher
2017-12-06 12:48:00 -08:00
parent 490307fe50
commit d914111e16
30 changed files with 470 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
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
------

4
apk/com.baseui.apk
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2617aa99a01fd0994e53b34a32699ac75186ac8a40c2e3021e4ce3ca2ba03de2
size 6335294
oid sha256:6a4fc195fb4160a445c9e5b1d1aa4e37bcf8c3271890352da8b33524893dfa1c
size 6338698

70
cereal/car.capnp
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
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
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 Normal file
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
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

Submodule opendbc updated: c8eeedce17...242698f800

6
selfdrive/car/honda/carcontroller.py
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
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
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
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
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
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
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;

4
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f01adf6d07d2ca818d2df6a79b1c10a4806dd6a1a525438950329ff0c2791437
size 194874
oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20
size 194688

142
selfdrive/controls/lib/planner.py
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
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:])
self.counters = defaultdict(lambda: 0)
@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
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)
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
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
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
# 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
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.vision_score = NO_FUSION_SCORE
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
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.
return d_path < 1.5 and not self.stationary and not self.oncoming
# consider only cut-ins
d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
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
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
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
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
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
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]

26
selfdrive/radar/toyota/interface.py
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['VALID'] and cpt['LONG_DIST'] < 255:
self.ptsValid[ii] = True
else:
self.ptsValid[ii] = False
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
self.validCnt[ii] = 0 # reset counter
if self.ptsValid[ii]:
#print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
self.validCnt[ii] += 1
else:
self.validCnt[ii] = max(self.validCnt[ii] -1, 0)
#print ii, self.validCnt[ii], cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
# 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
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
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
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
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
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);

4
selfdrive/visiond/visiond
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:9fe67ea99d1cc0e003fb68cb9c71bbc6ff0b0124b043bcb58ee4df43bbdb7547
size 13334208
oid sha256:008834e55cbf9b93d3bc017a2aa56ac1f431e31bc4fc8a5f904642f757245c5e
size 13334480