diff --git a/README.md b/README.md index f9a91e8f0394b1..f8de745a27186a 100644 --- a/README.md +++ b/README.md @@ -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 ------ diff --git a/apk/com.baseui.apk b/apk/com.baseui.apk index b07892e185844b..43c08418f3d406 100644 Binary files a/apk/com.baseui.apk and b/apk/com.baseui.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 1580aa437ad500..6b58254d8856fc 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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 } diff --git a/cereal/log.capnp b/cereal/log.capnp index fd550c2de9eaff..e1d61b88bc4c07 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py index 4fe7fc788cc9b9..454966d64d52a4 100644 --- a/common/kalman/ekf.py +++ b/common/kalman/ekf.py @@ -19,6 +19,7 @@ # 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 @@ def __repr__(self): # 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 @@ def update_scalar(self, reading): # 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 @@ def predict(self, dt): #! 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 @@ def calc_transfer_fun(self, dt): and using it during calcualtion of A and J """ + class FastEKF1D(EKF): """Fast version of EKF for 1D problems with scalar readings.""" diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py new file mode 100644 index 00000000000000..3f7d049cc55342 --- /dev/null +++ b/common/kalman/simple_kalman.py @@ -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 diff --git a/common/params.py b/common/params.py index a13095731f4757..4701608561db28 100755 --- a/common/params.py +++ b/common/params.py @@ -57,6 +57,7 @@ class UnknownKeyName(Exception): "IsMetric": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT, + "HasAcceptedTerms": TxType.PERSISTANT, # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTANT, diff --git a/opendbc b/opendbc index c8eeedce1717c6..242698f80038ba 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21 +Subproject commit 242698f80038bab677a4a6e58127309f9ed38d93 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 287deccb581170..fbf51f36a1bed3 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -11,10 +11,6 @@ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 90eca2b38dda3b..62bb834cbbe281 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -10,7 +10,6 @@ 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 @@ 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 @@ def __init__(self, CP, sendcan=None): 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 @@ def get_params(candidate, fingerprint): # 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 @@ def get_params(candidate, fingerprint): 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 @@ def get_params(candidate, fingerprint): 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 @@ def get_params(candidate, fingerprint): 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 @@ def get_params(candidate, fingerprint): # 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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3d8e4e170d1e88..fc75335361af9a 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -62,19 +62,19 @@ def get_params(candidate, fingerprint): # 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 @@ def get_params(candidate, fingerprint): 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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4a33a22c8133fb..75ace9cdbed3b0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 5c1ba67953d956..82455e114f1208 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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: diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index c8439a79a0cdde..afe87e952e98e1 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs 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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs 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() diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 36d8aea56b3e95..7c0484bfc9dd18 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -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; diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c index cbf07710a5dbf9..963a36430a9bb7 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0]; tmpQ2[1] = + tmpFx[4]; tmpQ2[2] = + tmpFx[8]; tmpQ2[3] = + tmpFx[12]; -tmpQ2[4] = + tmpFx[16]*(real_t)2.0000000000000000e+00; +tmpQ2[4] = + tmpFx[16]; tmpQ2[5] = + tmpFx[1]; tmpQ2[6] = + tmpFx[5]; tmpQ2[7] = + tmpFx[9]; tmpQ2[8] = + tmpFx[13]; -tmpQ2[9] = + tmpFx[17]*(real_t)2.0000000000000000e+00; +tmpQ2[9] = + tmpFx[17]; tmpQ2[10] = + tmpFx[2]; tmpQ2[11] = + tmpFx[6]; tmpQ2[12] = + tmpFx[10]; tmpQ2[13] = + tmpFx[14]; -tmpQ2[14] = + tmpFx[18]*(real_t)2.0000000000000000e+00; +tmpQ2[14] = + tmpFx[18]; tmpQ2[15] = + tmpFx[3]; tmpQ2[16] = + tmpFx[7]; tmpQ2[17] = + tmpFx[11]; tmpQ2[18] = + tmpFx[15]; -tmpQ2[19] = + tmpFx[19]*(real_t)2.0000000000000000e+00; +tmpQ2[19] = + tmpFx[19]; tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; @@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0]; tmpR2[1] = + tmpFu[1]; tmpR2[2] = + tmpFu[2]; tmpR2[3] = + tmpFu[3]; -tmpR2[4] = + tmpFu[4]*(real_t)2.0000000000000000e+00; +tmpR2[4] = + tmpFu[4]; tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; } @@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]; tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1]; tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2]; tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3]; -tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)2.0000000000000000e+00; +tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]; objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 67052ed4008980..bb6711e8b0e913 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -3,6 +3,7 @@ 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_TOTAL_MAX_V = [1.5, 1.9, 3.2] _A_TOTAL_MAX_BP = [0., 20., 40.] +_FCW_A_ACT_V = [-3., -2.] +_FCW_A_ACT_BP = [0., 30.] + # max acceleration allowed in acc, which happens in restart A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -61,7 +65,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): deg_to_rad = np.pi / 180. # from can reading to rad a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) - a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l) + a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.steerRatio * CP.wheelbase) a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) a_target[1] = min(a_target[1], a_x_allowed) @@ -70,36 +74,64 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class FCWChecker(object): def __init__(self): - self.fcw_count = 0 - self.last_fcw_a = 0.0 - self.v_lead_max = 0.0 - self.lead_seen_t = 0.0 - self.last_fcw_time = 0.0 + self.reset_lead(0.0) def reset_lead(self, cur_time): + self.last_fcw_a = 0.0 self.v_lead_max = 0.0 self.lead_seen_t = cur_time + self.last_fcw_time = 0.0 + self.last_min_a = 0.0 + + self.counters = defaultdict(lambda: 0) - def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers): - min_a_mpc = min(list(mpc_solution[0].a_ego)[1:]) + @staticmethod + def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead): + max_ttc = 5.0 + + v_rel = v_ego - v_lead + a_rel = a_ego - a_lead + + # assuming that closing gap ARel comes from lead vehicle decel, + # then limit ARel so that v_lead will get to zero in no sooner than t_decel. + # This helps underweighting ARel when v_lead is close to zero. + t_decel = 2. + a_rel = np.minimum(a_rel, v_lead/t_decel) + + # delta of the quadratic equation to solve for ttc + delta = v_rel**2 + 2 * x_lead * a_rel + + # assign an arbitrary high ttc value if there is no solution to ttc + if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1): + ttc = max_ttc + else: + ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) + return ttc + + def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): + mpc_solution_a = list(mpc_solution[0].a_ego) + self.last_min_a = min(mpc_solution_a[1:]) self.v_lead_max = max(self.v_lead_max, v_lead) - if (fcw_lead > 0.99 - and v_ego > 5.0 - and min_a_mpc < -4.0 - and self.v_lead_max > 2.5 - and v_ego > v_lead - and self.lead_seen_t < cur_time - 2.0 - and abs(y_lead) < 1.0 - and abs(vlat_lead) < 0.3 - and not blinkers): - self.fcw_count += 1 - if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time: + if (fcw_lead > 0.99): + ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead) + self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0 + self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0 + self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0 + self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0 + self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33 + self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0 + self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0 + self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0 + + a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V) + a_delta = min(mpc_solution_a[1:15]) - min(0.0, a_ego) + + fcw_allowed = all(c >= 10 for c in self.counters.values()) + if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time: self.last_fcw_time = cur_time - self.last_fcw_a = min_a_mpc + self.last_fcw_a = self.last_min_a return True - else: - self.fcw_count = 0 return False @@ -214,6 +246,8 @@ def update(self, CS, lead, v_cruise_setpoint): 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 @@ def __init__(self, CP, fcw_enabled): self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled - def choose_solution(self, v_cruise_setpoint): - solutions = {'cruise': self.v_cruise} - if self.mpc1.prev_lead_status: - solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status: - solutions['mpc2'] = self.mpc2.v_mpc - - slowest = min(solutions, key=solutions.get) - - if _DEBUG: - print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol - print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise - print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise - - self.longitudinalPlanSource = slowest - - # Choose lowest of MPC and cruise - if slowest == 'mpc1': - self.v_acc = self.mpc1.v_mpc - self.a_acc = self.mpc1.a_mpc - elif slowest == 'mpc2': - self.v_acc = self.mpc2.v_mpc - self.a_acc = self.mpc2.a_mpc - elif slowest == 'cruise': - self.v_acc = self.v_cruise - self.a_acc = self.a_cruise + def choose_solution(self, v_cruise_setpoint, enabled): + if enabled: + solutions = {'cruise': self.v_cruise} + if self.mpc1.prev_lead_status: + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status: + solutions['mpc2'] = self.mpc2.v_mpc + + slowest = min(solutions, key=solutions.get) + + if _DEBUG: + print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol + print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise + print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise + + self.longitudinalPlanSource = slowest + + # Choose lowest of MPC and cruise + if slowest == 'mpc1': + self.v_acc = self.mpc1.v_mpc + self.a_acc = self.mpc1.a_mpc + elif slowest == 'mpc2': + self.v_acc = self.mpc2.v_mpc + self.a_acc = self.mpc2.a_mpc + elif slowest == 'cruise': + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) @@ -331,19 +366,19 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): 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 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): 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: diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 15cc871e8cb662..6dbb72c5c40972 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -5,7 +5,9 @@ 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_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 @@ def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned): 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 @@ def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned): self.vLead = self.vRel + v_ego_t_aligned if not self.initted: + self.initted = True + self.cnt = 1 + self.vision_cnt = 0 + self.vision = False self.aRel = 0. # nidec gives no information about this self.vLat = 0. - self.aLead = 0. + self.kf = KF1D(np.matrix([[self.vLead], [0.0]]), _VLEAD_A, _VLEAD_C, _VLEAD_K) else: # estimate acceleration + # TODO: use Kalman filter a_rel_unfilt = (self.vRel - self.vRelPrev) / ts a_rel_unfilt = clip(a_rel_unfilt, -10., 10.) self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel - v_lat_unfilt = (self.dPath - self.dPathPrev) / ts + # TODO: use Kalman filter + # neglect steer override cases as dPath is too noisy + v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat - a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts - a_lead_unfilt = clip(a_lead_unfilt, -10., 10.) - self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead + self.kf.update(self.vLead) + + self.cnt += 1 + + self.vLeadK = float(self.kf.x[SPEED]) + self.aLeadK = float(self.kf.x[ACCEL]) if self.stationary: # stationary objects can become non stationary, but not the other way around self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr self.oncoming = self.vLead < v_oncoming_thr - if self.ekf is None: - self.ekf = FastEKF1D(ts, 1e3, [0.1, 1]) - self.ekf.state[SPEED] = self.vLead - self.ekf.state[ACCEL] = 0 - self.lead_sensor = SimpleSensor(SPEED, 1, 2) - - self.vLeadK = self.vLead - self.aLeadK = self.aLead - else: - self.ekf.update_scalar(self.lead_sensor.read(self.vLead)) - self.ekf.predict(ts) - self.vLeadK = float(self.ekf.state[SPEED]) - self.aLeadK = float(self.ekf.state[ACCEL]) - - if not self.initted: - self.cnt = 1 - self.vision_cnt = 0 - else: - self.cnt += 1 - - self.initted = True - self.vision = False + self.vision_score = NO_FUSION_SCORE - def mix_vision(self, dist_to_vision, rel_speed_diff): + def update_vision_score(self, dist_to_vision, rel_speed_diff): # rel speed is very hard to estimate from vision if dist_to_vision < 4.0 and rel_speed_diff < 10.: - # vision point is never stationary - self.vision_cnt += 1 - # don't trust 1 or 2 fusions until model quality is much better - if self.vision_cnt >= 3: - self.vision = True - self.stationary = False + self.vision_score = dist_to_vision + rel_speed_diff + else: + self.vision_score = NO_FUSION_SCORE + + def update_vision_fusion(self): + # vision point is never stationary + # don't trust 1 or 2 fusions until model quality is much better + if self.vision_cnt >= 3: + self.vision = True + self.stationary = False def get_key_for_cluster(self): # Weigh y higher since radar is inaccurate in this dimension @@ -159,10 +164,6 @@ def aRel(self): 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 @@ def aLeadK(self): 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 @@ def toLive20(self, lead): 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 @@ def is_potential_lead(self, v_ego): # lat_corr used to be gated on enabled, now always running t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v) - # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact - lat_corr = clip(t_lookahead * self.vLat, -1, 0) - d_path = max(d_path + lat_corr, 0) + # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact. + lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0. + + # consider only cut-ins + d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path)) - return d_path < 1.5 and not self.stationary and not self.oncoming + return abs(d_path) < 1.5 and not self.stationary and not self.oncoming def is_potential_lead2(self, lead_clusters): if len(lead_clusters) > 0: diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py index 50d3687376248f..d9bc0329d1f58b 100644 --- a/selfdrive/controls/lib/speed_smoother.py +++ b/selfdrive/controls/lib/speed_smoother.py @@ -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): diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 91e26e4ff4bde1..3cdb8a104b627c 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -10,36 +10,36 @@ # 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 @@ def __init__(self, CP, init_state=np.asarray([[0.], [0.]])): 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 @@ def steady_state_sol(self, sa, u): # 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) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 1e2eed8cd0095c..6dec7d1e105559 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -9,7 +9,8 @@ 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.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) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index ef83e88e569e6a..4771776cf1d399 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -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") diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index 00b02c2fc37e26..dca71f32f4d4db 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -33,6 +33,7 @@ def __init__(self): 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 @@ def update(self): 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] diff --git a/selfdrive/radar/toyota/interface.py b/selfdrive/radar/toyota/interface.py index 6b0554673ebe2e..40445f6686fe80 100755 --- a/selfdrive/radar/toyota/interface.py +++ b/selfdrive/radar/toyota/interface.py @@ -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 @@ def update(self): errors.append("commIssue") ret.errors = errors ret.canMonoTimes = canMonoTimes - #print "NEW TRACKS" + for ii in updated_messages: cpt = self.rcp.vl[ii] - # a point needs one valid measurement before being considered - #if cpt['NEW_TRACK'] or cpt['LONG_DIST'] >= 255: - # self.ptsValid[ii] = False # reset validity - # TODO: find better way to eliminate both false positive and false negative + if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']: + self.validCnt[ii] = 0 # reset counter + if cpt['VALID'] and cpt['LONG_DIST'] < 255: - self.ptsValid[ii] = True + self.validCnt[ii] += 1 else: - self.ptsValid[ii] = False + self.validCnt[ii] = max(self.validCnt[ii] -1, 0) + #print ii, self.validCnt[ii], cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST'] - if self.ptsValid[ii]: - #print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1)) + # radar point only valid if there have been enough valid measurements + if self.validCnt[ii] > 0: if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() self.pts[ii].trackId = self.track_id @@ -79,6 +80,7 @@ def update(self): 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] diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 5a408c99132c5f..92186600e42c4e 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -45,7 +45,7 @@ def evaluate(self): 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 @@ def evaluate(self): 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" diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/plant/maneuverplots.py index 365a13723802ae..0a3f3cd201cbb7 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/plant/maneuverplots.py @@ -33,11 +33,13 @@ def __init__(self, title = None): 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 @@ def add_data(self, time, gas, brake, steer_torque, distance, speed, 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 @@ def write_plot(self, path, maneuver_name): 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) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index d2208aeba1e641..43b0667726a2e8 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -102,6 +102,7 @@ def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0) 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 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) 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 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) 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 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) 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): diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index 9ffb247f8fc2b6..73380022b8a513 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -197,9 +197,51 @@ def create_dir(path): (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 @@ def setUpClass(cls): 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') diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index d93311118a9eea..e621af4117525c 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -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); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index d750726e135e7d..7a98efab048d92 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ