openpilot v0.2.1 release
Vehicle Researcher
parent
449b482cc3
commit
17d9becd3c
|
|
@ -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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include <string.h>
|
||||
#include <signal.h>
|
||||
#include <unistd.h>
|
||||
#include <sched.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/cdefs.h>
|
||||
#include <sys/types.h>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 ***
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
# TODO: not every time
|
||||
if VP.brake_only:
|
||||
gas_max_v = [0, 0] # values
|
||||
else:
|
||||
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_max_v = [1.0, 1.0, 0.8, 0.8] # values
|
||||
|
||||
# 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)
|
||||
brake_max = interp(v_ego, brake_max_bp, brake_max_v)
|
||||
|
||||
# TODO: not every time
|
||||
if VP.brake_only:
|
||||
gas_max = 0
|
||||
else:
|
||||
gas_max_bp = [0., 100.] # speeds
|
||||
gas_max_v = [0.6, 0.6] # values
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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 ***
|
||||
|
|
|
|||
Loading…
Reference in New Issue