diff --git a/LICENSE.openpilot b/LICENSE.openpilot index d1b0147269..8a6c6976b7 100644 --- a/LICENSE.openpilot +++ b/LICENSE.openpilot @@ -1,4 +1,4 @@ -Copyright (c) 2016, comma.ai +Copyright (c) 2016, Comma.ai, Inc. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: diff --git a/RELEASES.md b/RELEASES.md index d544b1ec7c..bf15ad0b1e 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,9 @@ +Version 0.2.1 (2016-12-14) +=========================== + * Performance improvements, removal of more numpy + * Fix boardd process priority + * Make counter timer reset on use of steering wheel + Version 0.2 (2016-12-12) ========================= * Car/Radar abstraction layers have shipped, see cereal/car.capnp diff --git a/common/dbc.py b/common/dbc.py index bc9dab3bac..4414b79839 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -1,6 +1,7 @@ import re -from collections import namedtuple import bitstring +from binascii import hexlify +from collections import namedtuple def int_or_float(s): # return number, trying to maintain int format @@ -148,8 +149,8 @@ class dbc(object): if debug: print name - blen = (len(x[2])/2)*8 - x2_int = int(x[2], 16) + blen = 8*len(x[2]) + x2_int = int(hexlify(x[2]), 16) for s in msg[1]: if arr is not None and s[0] not in arr: diff --git a/common/numpy_fast.py b/common/numpy_fast.py index bea164de0f..646a8bb1ab 100644 --- a/common/numpy_fast.py +++ b/common/numpy_fast.py @@ -1,2 +1,25 @@ def clip(x, lo, hi): return max(lo, min(hi, x)) + + +def interp(x, xp, fp): + N = len(xp) + if not hasattr(x, '__iter__'): + hi = 0 + while hi < N and x > xp[hi]: + hi += 1 + low = hi - 1 + return fp[-1] if hi == N and x > xp[low] else ( + fp[0] if hi == 0 else + (x - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) + + result = [] + for v in x: + hi = 0 + while hi < N and v > xp[hi]: + hi += 1 + low = hi - 1 + result.append(fp[-1] if hi == N and v > xp[low] else (fp[ + 0] if hi == 0 else (v - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low] + ) + fp[low])) + return result diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 5bcf7f754a..3cd0085022 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -268,12 +269,20 @@ void *can_health_thread(void *crap) { return NULL; } +int set_realtime_priority(int level) { + // should match python using chrt + struct sched_param sa; + memset(&sa, 0, sizeof(sa)); + sa.sched_priority = level; + return sched_setscheduler(gettid(), SCHED_FIFO, &sa); +} + int main() { int err; printf("boardd: starting boardd\n"); // set process priority - err = setpriority(PRIO_PROCESS, 0, -4); + err = set_realtime_priority(4); printf("boardd: setpriority returns %d\n", err); // check the environment diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index fecda964f6..17d4ba7f81 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -32,17 +32,11 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'): cc.src = can_msg[3] return dat -def can_capnp_to_can_list_old(dat, src_filter=[]): +def can_capnp_to_can_list(can, src_filter=None): ret = [] - for msg in dat: - if msg.src in src_filter: - ret.append([msg.address, msg.busTime, msg.dat.encode("hex")]) - return ret - -def can_capnp_to_can_list(dat): - ret = [] - for msg in dat.can: - ret.append([msg.address, msg.busTime, msg.dat, msg.src]) + for msg in can: + if src_filter is None or msg.src in src_filter: + ret.append((msg.address, msg.busTime, msg.dat, msg.src)) return ret # *** can driver *** diff --git a/selfdrive/car/honda/can_parser.py b/selfdrive/car/honda/can_parser.py index 7a28dece68..7275f1b83e 100644 --- a/selfdrive/car/honda/can_parser.py +++ b/selfdrive/car/honda/can_parser.py @@ -59,21 +59,21 @@ class CANParser(object): cn_vl_max = 5 # no more than 5 wrong counter checks # we are subscribing to PID_XXX, else data from USB - for msg, ts, cdat in can_recv: + for msg, ts, cdat, _ in can_recv: idxs = self._message_indices[msg] if idxs: - self.msgs_upd += [msg] + self.msgs_upd.append(msg) # read the entire message - out = self.can_dbc.decode([msg, 0, cdat])[1] + out = self.can_dbc.decode((msg, 0, cdat))[1] # checksum check self.ck[msg] = True if "CHECKSUM" in out.keys() and msg in self.msgs_ck: # remove checksum (half byte) - ck_portion = (''.join((cdat[:-1], '0'))).decode('hex') + ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0) # recalculate checksum msg_vl = fix(ck_portion, msg) # compare recalculated vs received checksum - if msg_vl != cdat.decode('hex'): + if msg_vl != cdat: print hex(msg), "CHECKSUM FAIL" self.ck[msg] = False self.ok[msg] = False diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 9404ddda81..1da53d1f56 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,7 +1,7 @@ import numpy as np import selfdrive.messaging as messaging -from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list +from selfdrive.boardd.boardd import can_capnp_to_can_list from selfdrive.config import VehicleParams from common.realtime import sec_since_boot @@ -132,7 +132,7 @@ def fingerprint(logcan): for a in messaging.drain_sock(logcan, wait_for_one=True): if st is None: st = sec_since_boot() - for adr, _, msg, idx in can_capnp_to_can_list(a): + for adr, _, msg, idx in can_capnp_to_can_list(a.can): # pedal if adr == 0x201 and idx == 0: brake_only = False diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9b561729cf..42866569f6 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -5,7 +5,7 @@ import numpy as np from selfdrive.config import Conversions as CV from selfdrive.car.honda.carstate import CarState from selfdrive.car.honda.carcontroller import CarController, AH -from selfdrive.boardd.boardd import can_capnp_to_can_list_old +from selfdrive.boardd.boardd import can_capnp_to_can_list from cereal import car @@ -42,6 +42,7 @@ class CarInterface(object): self.logcan = messaging.sub_sock(context, service_list['can'].port) self.frame = 0 + self.can_invalid_count = 0 # *** init the major players *** self.CS = CarState(self.logcan) @@ -61,7 +62,7 @@ class CarInterface(object): canMonoTimes = [] for a in messaging.drain_sock(self.logcan): canMonoTimes.append(a.logMonoTime) - can_pub_main.extend(can_capnp_to_can_list_old(a.can, [0,2])) + can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2])) self.CS.update(can_pub_main) # create message @@ -149,7 +150,11 @@ class CarInterface(object): # These strings aren't checked at compile time errors = [] if not self.CS.can_valid: - errors.append('commIssue') + self.can_invalid_count += 1 + if self.can_invalid_count >= 5: + errors.append('commIssue') + else: + self.can_invalid_count = 0 if self.CS.steer_error: errors.append('steerUnavailable') elif self.CS.steer_not_allowed: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 459c07e512..5c2da6b427 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -6,6 +6,8 @@ import selfdrive.messaging as messaging from cereal import car +from common.numpy_fast import clip + from selfdrive.config import Conversions as CV from common.services import service_list from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper @@ -73,7 +75,7 @@ def controlsd_thread(gctx, rate=100): #rate in Hz # start the loop set_realtime_priority(2) - rk = Ratekeeper(rate) + rk = Ratekeeper(rate, print_delay_threshold=2./1000) while 1: cur_time = sec_since_boot() @@ -89,6 +91,10 @@ def controlsd_thread(gctx, rate=100): #rate in Hz if awareness_status <= 0.: AM.add("driverDistracted", enabled) + # reset awareness status on steering + if CS.steeringPressed: + awareness_status = 1.0 + # handle button presses for b in CS.buttonEvents: print b @@ -111,7 +117,7 @@ def controlsd_thread(gctx, rate=100): #rate in Hz v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA elif b.type == "decelCruise": v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA - v_cruise_kph = np.clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) + v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed: enable_request = True @@ -185,7 +191,7 @@ def controlsd_thread(gctx, rate=100): #rate in Hz enabled = True # on activation, let's always set v_cruise from where we are, even if PCM ACC is active - v_cruise_kph = int(round(np.maximum(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN))) + v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN))) # 6 minutes driver you're on awareness_status = 1.0 diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py index bf940e0e1b..8d066080cb 100644 --- a/selfdrive/controls/lib/adaptivecruise.py +++ b/selfdrive/controls/lib/adaptivecruise.py @@ -1,41 +1,41 @@ -import selfdrive.messaging as messaging +import math import numpy as np +from common.numpy_fast import clip, interp +import selfdrive.messaging as messaging # lookup tables VS speed to determine min and max accels in cruise -_A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30]) -_A_CRUISE_MIN_BP = np.asarray([ 0., 5., 10., 20., 40.]) +_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go -_A_CRUISE_MAX_V = np.asarray([1., 1., .8, .5, .30]) -_A_CRUISE_MAX_BP = np.asarray([0., 5., 10., 20., 40.]) +_A_CRUISE_MAX_V = [1., 1., .8, .5, .30] +_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] def calc_cruise_accel_limits(v_ego): - a_cruise_min = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) - a_cruise_max = np.interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + return np.vstack([a_cruise_min, a_cruise_max]) - a_pcm = 1. # always 1 for now - return np.vstack([a_cruise_min, a_cruise_max]), a_pcm - -_A_TOTAL_MAX_V = np.asarray([1.5, 1.9, 3.2]) -_A_TOTAL_MAX_BP = np.asarray([0., 20., 40.]) +_A_TOTAL_MAX_V = [1.5, 1.9, 3.2] +_A_TOTAL_MAX_BP = [0., 20., 40.] def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP): #*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration # this should avoid accelerating when losing the target in turns deg_to_rad = np.pi / 180. # from can reading to rad - a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase) - a_x_allowed = np.sqrt(np.maximum(a_total_max**2 - a_y**2, 0.)) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) - a_target[1] = np.minimum(a_target[1], a_x_allowed) - a_pcm = np.minimum(a_pcm, a_x_allowed) + a_target[1] = min(a_target[1], a_x_allowed) + a_pcm = min(a_pcm, a_x_allowed) return a_target, a_pcm def process_a_lead(a_lead): # soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead a_lead_threshold = 0.5 - a_lead = np.minimum(a_lead + a_lead_threshold, 0) + a_lead = min(a_lead + a_lead_threshold, 0) return a_lead def calc_desired_distance(v_lead): @@ -46,12 +46,12 @@ def calc_desired_distance(v_lead): #linear slope -_L_SLOPE_V = np.asarray([0.40, 0.10]) -_L_SLOPE_BP = np.asarray([0., 40]) +_L_SLOPE_V = [0.40, 0.10] +_L_SLOPE_BP = [0., 40] # parabola slope -_P_SLOPE_V = np.asarray([1.0, 0.25]) -_P_SLOPE_BP = np.asarray([0., 40]) +_P_SLOPE_V = [1.0, 0.25] +_P_SLOPE_BP = [0., 40] def calc_desired_speed(d_lead, d_des, v_lead, a_lead): #*** compute desired speed *** @@ -64,8 +64,8 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead): max_runaway_speed = -2. # no slower than 2m/s over the lead # interpolate the lookups to find the slopes for a give lead speed - l_slope = np.interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V) - p_slope = np.interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V) + l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V) + p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V) # this is where parabola and linear curves are tangents x_linear_to_parabola = p_slope / l_slope**2 @@ -79,41 +79,41 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead): # calculate v_rel_des on one third of the linear slope v_rel_des_2 = (d_lead - d_des) * l_slope / 3. # take the min of the 2 above - v_rel_des = np.minimum(v_rel_des_1, v_rel_des_2) - v_rel_des = np.maximum(v_rel_des, max_runaway_speed) + v_rel_des = min(v_rel_des_1, v_rel_des_2) + v_rel_des = max(v_rel_des, max_runaway_speed) elif d_lead < d_des + x_linear_to_parabola: v_rel_des = (d_lead - d_des) * l_slope - v_rel_des = np.maximum(v_rel_des, max_runaway_speed) + v_rel_des = max(v_rel_des, max_runaway_speed) else: - v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope) + v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope) # compute desired speed v_target = v_rel_des + v_lead # compute v_coast: above this speed we want to coast t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region - v_coast_shift = np.maximum(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0 + v_coast_shift = max(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0 v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line - v_coast = np.minimum(v_coast, v_target) + v_coast = min(v_coast, v_target) return v_target, v_coast def calc_critical_decel(d_lead, v_rel, d_offset, v_offset): # this function computes the required decel to avoid crashing, given safety offsets - a_critical = - np.maximum(0., v_rel + v_offset)**2/np.maximum(2*(d_lead - d_offset), 0.5) + a_critical = - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5) return a_critical # maximum acceleration adjustment -_A_CORR_BY_SPEED_V = np.asarray([0.4, 0.4, 0]) +_A_CORR_BY_SPEED_V = [0.4, 0.4, 0] # speeds -_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.]) +_A_CORR_BY_SPEED_BP = [0., 5., 20.] def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max): a_coast_min = -1.0 # never coast faster then -1m/s^2 # coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease, # regardless v_target - if v_ref > np.minimum(v_coast, v_target): + if v_ref > min(v_coast, v_target): # for smooth coast we can be agrressive and target a point where car would actually crash v_offset_coast = 0. d_offset_coast = d_des/2. - 4. @@ -123,31 +123,31 @@ def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_c a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast) # if lead is decelerating, then offset the coast decel a_coast += a_lead_contr - a_max = np.maximum(a_coast, a_coast_min) + a_max = max(a_coast, a_coast_min) else: a_max = a_coast_min else: # same as cruise accel, but add a small correction based on lead acceleration at low speeds # when lead car accelerates faster, we can do the same, and vice versa - a_max = a_max + np.interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \ - * np.clip(-v_rel / 4., -.5, 1) + a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \ + * clip(-v_rel / 4., -.5, 1) return a_max # arbitrary limits to avoid too high accel being computed -_A_SAT = np.asarray([-10., 5.]) +_A_SAT = [-10., 5.] # do not consider a_lead at 0m/s, fully consider it at 10m/s -_A_LEAD_LOW_SPEED_V = np.asarray([0., 1.]) +_A_LEAD_LOW_SPEED_V = [0., 1.] # speed break points -_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.]) +_A_LEAD_LOW_SPEED_BP = [0., 10.] # add a small offset to the desired decel, just for safety margin -_DECEL_OFFSET_V = np.asarray([-0.3, -0.5, -0.5, -0.4, -0.3]) +_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3] # speed bp: different offset based on the likelyhood that lead decels abruptly -_DECEL_OFFSET_BP = np.asarray([0., 4., 15., 30, 40.]) +_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.] def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead, @@ -159,8 +159,8 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead, v_rel_pid = v_pid - v_lead # this is how much lead accel we consider in assigning the desired decel - a_lead_contr = a_lead * np.interp(v_lead, _A_LEAD_LOW_SPEED_BP, - _A_LEAD_LOW_SPEED_V) * 0.8 + a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP, + _A_LEAD_LOW_SPEED_V) * 0.8 # first call of calc_positive_accel_limit is used to shape v_pid a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid, @@ -178,15 +178,15 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead, pass # acc target speed is above vehicle speed, so we can use the cruise limits elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions # compute needed accel to get to 1m distance with -1m/s rel speed - decel_offset = np.interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V) + decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V) critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset) - a_target[0] = np.minimum(decel_offset + critical_decel + a_lead_contr, - a_target[0]) + a_target[0] = min(decel_offset + critical_decel + a_lead_contr, + a_target[0]) else: a_target[0] = _A_SAT[0] # a_min can't be higher than a_max - a_target[0] = np.minimum(a_target[0], a_target[1]) + a_target[0] = min(a_target[0], a_target[1]) # final check on limits a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1]) a_target = a_target.tolist() @@ -208,8 +208,8 @@ def calc_jerk_factor(d_lead, v_rel): else: a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset) # increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2 - jerk_factor = np.maximum(a_critical - a_offset, 0.)/5. - jerk_factor = np.minimum(jerk_factor, jerk_factor_max) + jerk_factor = max(a_critical - a_offset, 0.)/5. + jerk_factor = min(jerk_factor, jerk_factor_max) return jerk_factor @@ -223,27 +223,27 @@ def calc_ttc(d_rel, v_rel, a_rel, v_lead): # assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel # this helps overweighting a_rel when v_lead is close to zero. t_decel = 2. - a_rel = np.minimum(a_rel, v_lead/t_decel) + a_rel = min(a_rel, v_lead/t_decel) delta = v_rel**2 + 2 * d_rel * a_rel # assign an arbitrary high ttc value if there is no solution to ttc if delta < 0.1: ttc = 5. - elif np.sqrt(delta) + v_rel < 0.1: + elif math.sqrt(delta) + v_rel < 0.1: ttc = 5. else: - ttc = 2 * d_rel / (np.sqrt(delta) + v_rel) + ttc = 2 * d_rel / (math.sqrt(delta) + v_rel) return ttc def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status): decel_bp = [0. , 40.] decel_v = [-0.3, -0.2] - decel = np.interp(v_ego, decel_bp, decel_v) + decel = interp(v_ego, decel_bp, decel_v) # gives 18 seconds before decel begins (w 6 minute timeout) if awareness_status < -0.05: - a_target[1] = np.minimum(a_target[1], decel) - a_target[0] = np.minimum(a_target[1], a_target[0]) + a_target[1] = min(a_target[1], decel) + a_target[0] = min(a_target[1], a_target[0]) a_pcm = 0. return a_target, a_pcm @@ -258,7 +258,10 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_statu v_target_lead = MAX_SPEED_POSSIBLE #*** set accel limits as cruise accel/decel limits *** - a_target, a_pcm = calc_cruise_accel_limits(v_ego) + a_target = calc_cruise_accel_limits(v_ego) + # Always 1 for now. + a_pcm = 1 + #*** limit max accel in sharp turns a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP) jerk_factor = 0. diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a2c4917dc1..bfa6ca74f1 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,7 +1,8 @@ import numpy as np +from common.numpy_fast import clip, interp def rate_limit(new_value, last_value, dw_step, up_step): - return np.clip(new_value, last_value + dw_step, last_value + up_step) + return clip(new_value, last_value + dw_step, last_value + up_step) def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight @@ -11,12 +12,12 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, stee min_learn_speed = 1. # learn less at low speed or when turning - alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des)) + alpha_v = alpha*(max(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des)) # only learn if lateral control is active and if driver is not overriding: if lateral_control and not steer_override: angle_offset += d_poly[3] * alpha_v - angle_offset = np.clip(angle_offset, min_offset, max_offset) + angle_offset = clip(angle_offset, min_offset, max_offset) return angle_offset @@ -44,7 +45,7 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic): brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds # offset the brake command for threshold in the brake system. no brake torque perceived below it - brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v) + brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v) brake_offset = brake_on_offset - brake_hyst_on if final_brake > 0.0: final_brake += brake_offset diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index cffbc8fa77..b7b5c75b3a 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,4 +1,6 @@ +import math import numpy as np +from common.numpy_fast import clip def calc_curvature(v_ego, angle_steers, VP, angle_offset=0): deg_to_rad = np.pi/180. @@ -14,7 +16,7 @@ def calc_d_lookahead(v_ego): # sqrt on speed is needed to keep, for a given curvature, the y_offset # proportional to speed. Indeed, y_offset is prop to d_lookahead^2 # 26m at 25m/s - d_lookahead = offset_lookahead + np.sqrt(np.maximum(v_ego, 0)) * coeff_lookahead + d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead return d_lookahead def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset): @@ -53,7 +55,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max, Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer) # still, intergral term should not be bigger then limits - Ui_steer = np.clip(Ui_steer, -steer_max, steer_max) + Ui_steer = clip(Ui_steer, -steer_max, steer_max) output_steer = Up_steer + Ui_steer @@ -67,7 +69,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max, if abs(output_steer) > steer_max: lateral_control_sat = True - output_steer = np.clip(output_steer, -steer_max, steer_max) + output_steer = clip(output_steer, -steer_max, steer_max) # if lateral control is saturated for a certain period of time, send an alert for taking control of the car # wind @@ -81,7 +83,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max, if sat_count >= sat_count_limit: sat_flag = True - sat_count = np.clip(sat_count, 0, 1) + sat_count = clip(sat_count, 0, 1) return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag @@ -116,5 +118,5 @@ class LatControl(object): v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max, steer_override, self.sat_count, enabled, VP.torque_mod, rate) - final_steer = np.clip(output_steer, -steer_max, steer_max) + final_steer = clip(output_steer, -steer_max, steer_max) return final_steer, sat_flag diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 23ff8362b5..d6e1c8e819 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,4 +1,5 @@ import numpy as np +from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV class LongCtrlState: @@ -82,15 +83,18 @@ def get_compute_gb(): # takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas compute_gb = get_compute_gb() + +_KP_BP = [0., 5., 35.] +_KP_V = [1.2, 0.8, 0.5] + +_kI_BP = [0., 35.] +_kI_V = [0.18, 0.12] + def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate): #*** This function compute the gb pedal positions in order to track the desired speed # proportional and integral terms. More precision at low speed - Kp_v = [1.2, 0.8, 0.5] - Kp_bp = [0., 5., 35.] - Kp = np.interp(v_ego, Kp_bp, Kp_v) - Ki_v = [0.18, 0.12] - Ki_bp = [0., 35.] - Ki = np.interp(v_ego, Ki_bp, Ki_v) + Kp = interp(v_ego, _KP_BP, _KP_V) + Ki = interp(v_ego, _kI_BP, _kI_V) # scle Kp and Ki by jerk factor drom drive_thread Kp = (1. + jerk_factor)*Kp @@ -98,7 +102,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor # this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump v_ego_min = 0.3 - v_ego = np.maximum(v_ego, v_ego_min) + v_ego = max(v_ego, v_ego_min) v_error = v_pid - v_ego @@ -126,7 +130,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor if output_gb > gas_max or output_gb < -brake_max: long_control_sat = True - output_gb = np.clip(output_gb, -brake_max, gas_max) + output_gb = clip(output_gb, -brake_max, gas_max) return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat @@ -136,8 +140,8 @@ starting_brake_rate = 0.6 # brake_travel/s while releasing on restart starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary -max_speed_error_v = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp -max_speed_error_bp = [0., 30.] # speed breakpoints +_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp class LongControl(object): def __init__(self): @@ -152,18 +156,19 @@ class LongControl(object): self.v_pid = v_pid def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, VP): + brake_max_bp = [0., 5., 20., 100.] # speeds + brake_max_v = [1.0, 1.0, 0.8, 0.8] # values + + # brake and gas limits + brake_max = interp(v_ego, brake_max_bp, brake_max_v) + # TODO: not every time if VP.brake_only: - gas_max_v = [0, 0] # values + gas_max = 0 else: + gas_max_bp = [0., 100.] # speeds gas_max_v = [0.6, 0.6] # values - gas_max_bp = [0., 100.] # speeds - brake_max_v = [1.0, 1.0, 0.8, 0.8] # values - brake_max_bp = [0., 5., 20., 100.] # speeds - - # brake and gas limits - brake_max = np.interp(v_ego, brake_max_bp, brake_max_v) - gas_max = np.interp(v_ego, gas_max_bp, gas_max_v) + gas_max = interp(v_ego, gas_max_bp, gas_max_v) overshoot_allowance = 2.0 # overshoot allowed when changing accel sign @@ -172,7 +177,7 @@ class LongControl(object): # limit max target speed based on cruise setting: v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC - v_target = np.minimum(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / VP.ui_speed_fudge) + v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / VP.ui_speed_fudge) max_speed_delta_up = a_target[1]*1.0/rate max_speed_delta_down = a_target[0]*1.0/rate @@ -192,10 +197,10 @@ class LongControl(object): #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)): - self.v_pid = np.maximum(v_target, v_ego + overshoot_allowance) + self.v_pid = max(v_target, v_ego + overshoot_allowance) elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)): - self.v_pid = np.minimum(v_target, v_ego - overshoot_allowance) + self.v_pid = min(v_target, v_ego - overshoot_allowance) # move v_pid no faster than allowed accel limits if (v_target > self.v_pid + max_speed_delta_up): @@ -207,8 +212,8 @@ class LongControl(object): # to avoid too much wind up on acceleration, limit positive speed error if not VP.brake_only: - max_speed_error = np.interp(v_ego, max_speed_error_bp, max_speed_error_v) - self.v_pid = np.minimum(self.v_pid, v_ego + max_speed_error) + max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V) + self.v_pid = min(self.v_pid, v_ego + max_speed_error) # TODO: removed anti windup on gear change, does it matter? output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \ @@ -217,7 +222,7 @@ class LongControl(object): elif self.long_control_state == LongCtrlState.stopping: if v_ego > 0. or output_gb > -brake_stopping_target: output_gb -= stopping_brake_rate/rate - output_gb = np.clip(output_gb, -brake_max, gas_max) + output_gb = clip(output_gb, -brake_max, gas_max) self.v_pid = v_ego self.Ui_accel_cmd = 0. # intention is to move again, release brake fast before handling control to PID @@ -228,7 +233,7 @@ class LongControl(object): self.Ui_accel_cmd = starting_Ui self.last_output_gb = output_gb - final_gas = np.clip(output_gb, 0., gas_max) - final_brake = -np.clip(output_gb, -brake_max, 0.) + final_gas = clip(output_gb, 0., gas_max) + final_brake = -clip(output_gb, -brake_max, 0.) return final_gas, final_brake diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 49e5eb2e3e..3fba88561a 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,26 +1,29 @@ -import selfdrive.messaging as messaging +import math import numpy as np + +from common.numpy_fast import interp +import selfdrive.messaging as messaging X_PATH = np.arange(0.0, 50.0) def model_polyfit(points): return np.polyfit(X_PATH, map(float, points), 3) # lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm -_LANE_WIDTH_V = np.asarray([3., 3.8]) +_LANE_WIDTH_V = [3., 3.8] # break points of speed -_LANE_WIDTH_BP = np.asarray([0., 31.]) +_LANE_WIDTH_BP = [0., 31.] def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed): #*** this function computes the poly for the center of the lane, averaging left and right polys - lane_width = np.interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V) + lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V) # lanes in US are ~3.6m wide half_lane_poly = np.array([0., 0., 0., lane_width / 2.]) if l_prob + r_prob > 0.01: c_poly = ((l_poly - half_lane_poly) * l_prob + (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob) - c_prob = np.sqrt((l_prob**2 + r_prob**2) / 2.) + c_prob = math.sqrt((l_prob**2 + r_prob**2) / 2.) else: c_poly = np.zeros(4) c_prob = 0. diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index f7759a4d4d..ec3a68c464 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,8 +1,10 @@ -import numpy as np -import platform import os import sys +import math +import platform +import numpy as np +from common.numpy_fast import clip, interp from common.kalman.ekf import FastEKF1D, SimpleSensor # radar tracks @@ -51,14 +53,14 @@ class Track(object): else: # estimate acceleration a_rel_unfilt = (self.vRel - self.vRelPrev) / ts - a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.) + a_rel_unfilt = clip(a_rel_unfilt, -10., 10.) self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel v_lat_unfilt = (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 = np.clip(a_lead_unfilt, -10., 10.) + a_lead_unfilt = clip(a_lead_unfilt, -10., 10.) self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead if self.stationary: @@ -232,12 +234,12 @@ class Cluster(object): d_path = self.dPath if enabled: - t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v) + t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v) # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact - lat_corr = np.clip(t_lookahead * self.vLat, -1, 0) + lat_corr = clip(t_lookahead * self.vLat, -1, 0) else: lat_corr = 0. - d_path = np.maximum(d_path + lat_corr, 0) + d_path = max(d_path + lat_corr, 0) if d_path < 1.5 and not self.stationary and not self.oncoming: return True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bfaace6036..ae612f9aa9 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,7 +8,6 @@ from collections import defaultdict from fastcluster import linkage_vector import selfdrive.messaging as messaging -from selfdrive.boardd.boardd import can_capnp_to_can_list_old from selfdrive.controls.lib.latcontrol import calc_lookahead_offset from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.config import VehicleParams @@ -37,7 +36,6 @@ class EKFV1D(EKF): self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point self.covar = self.identity * self.var_init - # self.process_noise = np.asmatrix(np.diag([100, 10])) self.process_noise = np.matlib.diag([0.5, 1]) def calc_transfer_fun(self, dt): diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 128c47a61e..fb4fff2341 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -282,8 +282,15 @@ def main(): del managed_processes['loggerd'] if os.getenv("NOUPLOAD") is not None: del managed_processes['uploader'] + if os.getenv("NOVISION") is not None: + del managed_processes['visiond'] if os.getenv("NOBOARD") is not None: del managed_processes['boardd'] + if os.getenv("LEAN") is not None: + del managed_processes['uploader'] + del managed_processes['loggerd'] + del managed_processes['logmessaged'] + del managed_processes['logcatd'] manager_init() diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index 8c807b6400..061b341494 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import numpy as np from selfdrive.car.honda.can_parser import CANParser -from selfdrive.boardd.boardd import can_capnp_to_can_list_old +from selfdrive.boardd.boardd import can_capnp_to_can_list from cereal import car @@ -39,7 +39,7 @@ class RadarInterface(object): while 1: for a in messaging.drain_sock(self.logcan, wait_for_one=True): canMonoTimes.append(a.logMonoTime) - can_pub_radar.extend(can_capnp_to_can_list_old(a.can, [1, 3])) + can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) # only run on the 0x445 packets, used for timing if any(x[0] == 0x445 for x in can_pub_radar): diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/plant/maneuverplots.py index 0adac7ca5d..4e163103b7 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/plant/maneuverplots.py @@ -59,6 +59,7 @@ class ManeuverPlot(object): self.a_target_min_array.append(a_target_min) self.a_target_max_array.append(a_target_max) + def write_plot(self, path, maneuver_name): title = self.title or maneuver_name # TODO: Missing plots from the old one: diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 71e50c888f..57f9b815c9 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -14,7 +14,7 @@ import selfdrive.messaging as messaging from selfdrive.config import CruiseButtons from selfdrive.car.honda.hondacan import fix from selfdrive.car.honda.carstate import get_can_parser -from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list, can_list_to_can_capnp +from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp from selfdrive.car.honda.can_parser import CANParser @@ -141,8 +141,7 @@ class Plant(object): # ******** get messages sent to the car ******** can_msgs = [] for a in messaging.drain_sock(Plant.sendcan): - can_msgs += can_capnp_to_can_list_old(a.sendcan, [0,2]) - #print can_msgs + can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2])) self.cp.update_can(can_msgs) # ******** get live100 messages for plotting ***