def __init__(self, CP, fcw_enabled):
context = zmq.Context()
self.CP = CP
self.poller = zmq.Poller()
self.live20 = messaging.sub_sock(context,
service_list['live20'].port,
conflate=True,
poller=self.poller)
self.model = messaging.sub_sock(context,
service_list['model'].port,
conflate=True,
poller=self.poller)
if os.environ.get('GPS_PLANNER_ACTIVE', False):
self.gps_planner_plan = messaging.sub_sock(
context,
service_list['gpsPlannerPlan'].port,
conflate=True,
poller=self.poller,
addr=GPS_PLANNER_ADDR)
else:
self.gps_planner_plan = None
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
context, service_list['liveLongitudinalMpc'].port)
self.last_md_ts = 0
self.last_l20_ts = 0
self.last_model = 0.
self.last_l20 = 0.
self.model_dead = True
self.radar_dead = True
self.radar_errors = []
self.PP = PathPlanner()
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.acc_start_time = sec_since_boot()
self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.lead_1 = None
self.lead_2 = None
self.longitudinalPlanSource = 'cruise'
self.fcw = False
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.last_gps_planner_plan = None
self.gps_planner_active = False
def __init__(self, CP, fcw_enabled):
context = zmq.Context()
self.CP = CP
self.poller = zmq.Poller()
self.lat_Control = messaging.sub_sock(context,
service_list['latControl'].port,
conflate=True,
poller=self.poller)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
context, service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.lastlat_Control = None
self.params = Params()
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.path_x = np.arange(192)
self.params = Params()
def main(gctx=None):
global gpsLocationExternal, ubloxGnss
nav_frame_buffer = {}
nav_frame_buffer[0] = {}
for i in range(1,33):
nav_frame_buffer[0][i] = {}
if not CarSettings().get_value("useTeslaGPS"):
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
ubloxGnss = messaging.pub_sock('ubloxGnss')
dev = init_reader()
while True:
try:
msg = dev.receive_message()
except serial.serialutil.SerialException as e:
print(e)
dev.close()
dev = init_reader()
if msg is not None:
handle_msg(dev, msg, nav_frame_buffer)
else:
while True:
time.sleep(1.1)
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
self.model_v_kf_ready = False
self.params = Params()
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# pub
self.live100 = messaging.pub_sock(context, service_list['live100'].port)
self.carstate = messaging.pub_sock(context, service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = bool(params.get("IsRearViewMirror"))
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def steer_thread():
context = zmq.Context()
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
CI, CP = get_car(logcan, sendcan, None)
print("got car", CP.carName)
CC = car.CarControl.new_message()
i = 0
rate = 0.001
direction = 1
while True:
# send
CS = CI.update(CC)
actuators = car.CarControl.Actuators.new_message()
if i > 0.9 and direction == 1:
direction = -1
if i < -0.9 and direction == -1:
direction = 1
i += rate * direction
axis_3 = clip(i * 1.05, -1., 1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
print("steer", actuators.steer)
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.enabled = True
CI.apply(CC)
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
# Limit to 100 frames per second
time.sleep(0.01)
def radard_thread(gctx=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked = CP.carName == "mock"
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
can_sock = messaging.sub_sock(service_list['can'].port)
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
RI = RadarInterface(CP)
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining * 1000.
radarState.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def __init__(self,carstate):
self.CS = carstate
self.uiCustomAlert = messaging.pub_sock('uiCustomAlert')
self.uiButtonInfo = messaging.pub_sock('uiButtonInfo')
self.uiSetCar = messaging.pub_sock('uiSetCar')
self.uiPlaySound = messaging.pub_sock('uiPlaySound')
self.uiGyroInfo = messaging.pub_sock('uiGyroInfo')
self.uiButtonStatus = messaging.sub_sock('uiButtonStatus', conflate=True)
self.prev_cstm_message = ""
self.prev_cstm_status = -1
def __init__(self, CP):
self.MP = ModelParser()
self.last_cloudlog_t = 0
context = zmq.Context()
self.plan = messaging.pub_sock(context, service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
self.setup_mpc(CP.steerRateCost)
self.invalid_counter = 0
def __init__(self, CP):
self.LP = LanePlanner(shouldUseAlca=True)
self.last_cloudlog_t = 0
self.plan = messaging.pub_sock(service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
def __init__(self,carstate):
self.CS = carstate
context = zmq.Context()
self.buttons_poller = zmq.Poller()
self.uiCustomAlert = messaging.pub_sock(context, service_list['uiCustomAlert'].port)
self.uiButtonInfo = messaging.pub_sock(context, service_list['uiButtonInfo'].port)
self.uiSetCar = messaging.pub_sock(context, service_list['uiSetCar'].port)
self.uiPlaySound = messaging.pub_sock(context, service_list['uiPlaySound'].port)
self.uiButtonStatus = messaging.sub_sock(context, service_list['uiButtonStatus'].port, conflate=True, poller=self.buttons_poller)
self.prev_cstm_message = ""
self.prev_cstm_status = -1
def __init__(self,
lead_relevancy=False,
rate=100,
speed=0.0,
distance_lead=2.0):
self.rate = rate
self.civic = True
self.brake_only = False
if not Plant.messaging_initialized:
context = zmq.Context()
Plant.logcan = messaging.pub_sock(context,
service_list['can'].port)
Plant.sendcan = messaging.sub_sock(context,
service_list['sendcan'].port)
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.
self.gear_choice = 0
self.speed, self.speed_prev = 0., 0.
self.esp_disabled = 0
self.main_on = 1
self.user_gas = 0
self.computer_brake, self.user_brake = 0, 0
self.brake_pressed = 0
self.angle_steer_rate = 0
self.distance, self.distance_prev = 0., 0.
self.speed, self.speed_prev = speed, speed
self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
self.gear_shifter = 8 # D gear
self.pedal_gas = 0
self.cruise_setting = 0
self.seatbelt, self.door_all_closed = True, True
self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0 # v_cruise is reported from can, not the one used for controls
self.lead_relevancy = lead_relevancy
# lead car
self.distance_lead, self.distance_lead_prev = distance_lead, distance_lead
self.rk = Ratekeeper(rate, print_delay_threshold=100)
self.ts = 1. / rate
self.cp = get_car_can_parser()
def __init__(self, CP):
self.MP = ModelParser()
self.l_poly = [0., 0., 0., 0.]
self.r_poly = [0., 0., 0., 0.]
self.last_cloudlog_t = 0
self.plan = messaging.pub_sock(service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
def __init__(self, mocked, RI,use_tesla_radar, delay=0):
self.current_time = 0
self.mocked = mocked
self.RI = RI
self.tracks = defaultdict(dict)
self.last_md_ts = 0
self.last_controls_state_ts = 0
self.active = 0
# v_ego
self.v_ego = 0.
self.v_ego_hist = deque([0], maxlen=delay+1)
self.v_ego_t_aligned = 0.
self.ready = False
self.icCarLR = None
self.use_tesla_radar = use_tesla_radar
if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE) and self.use_tesla_radar:
self.icCarLR = messaging.pub_sock('uiIcCarLR')
self.lane_width = 3.0
#only used for left and right lanes
self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max
self.dPoly = [0.,0.,0.,0.]
def main():
context = zmq.Context()
rcv = sub_sock(context, service_list['can'].port) # port 8006
snd = pub_sock(context, service_list['sendcan'].port) # port 8017
time.sleep(0.3) # wait to bind before send/recv
for _ in range(10):
at = random.randint(1024, 2000)
st = get_test_string()[0:8]
snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes())
time.sleep(0.1)
res = drain_sock(rcv, True)
assert len(res) == 1
res = res[0].can
assert len(res) == 2
msg0, msg1 = res
assert msg0.dat == st
assert msg1.dat == st
assert msg0.address == at
assert msg1.address == at
assert msg0.src == 0x80 | BUS
assert msg1.src == BUS
print("Success")
def main(gctx):
# setup logentries. we forward log messages to it
le_token = "bc65354a-b887-4ef4-8525-15dd51230e8c"
le_handler = LogentriesHandler(le_token, use_tls=False)
le_level = 20 #logging.INFO
ctx = zmq.Context()
sock = ctx.socket(zmq.PULL)
sock.bind("ipc:///tmp/logmessage")
# and we publish them
pub_sock = messaging.pub_sock(ctx, service_list['logMessage'].port)
while True:
dat = ''.join(sock.recv_multipart())
# print "RECV", repr(dat)
levelnum = ord(dat[0])
dat = dat[1:]
if levelnum >= le_level:
# push to logentries
le_handler.emit_raw(dat)
# then we publish them
msg = messaging.new_message()
msg.logMessage = dat
pub_sock.send(msg.to_bytes())
def speedlimitd_thread():
context = zmq.Context()
gps_sock = messaging.sub_sock(context,
service_list['gpsLocationExternal'].port,
conflate=True)
speedlimit_sock = messaging.pub_sock(context,
service_list['speedLimit'].port)
while True:
gps = messaging.recv_sock(gps_sock)
if gps is not None:
fix_ok = gps.gpsLocationExternal.flags & 1
if fix_ok:
lat = gps.gpsLocationExternal.latitude
lon = gps.gpsLocationExternal.longitude
try:
max_speed = get_max_speed(lat, lon)
dat = ui.SpeedLimitData.new_message()
if max_speed:
dat.speed = max_speed * CV.MPH_TO_MS
speedlimit_sock.send(dat.to_bytes())
except Exception as e:
print(e)
def main(gctx=None):
# setup logentries. we forward log messages to it
le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17"
le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False)
le_level = 20 #logging.INFO
ctx = zmq.Context().instance()
sock = ctx.socket(zmq.PULL)
sock.bind("ipc:///tmp/logmessage")
# and we publish them
pub_sock = messaging.pub_sock(service_list['logMessage'].port)
while True:
dat = b''.join(sock.recv_multipart())
dat = dat.decode('utf8')
# print "RECV", repr(dat)
levelnum = ord(dat[0])
dat = dat[1:]
if levelnum >= le_level:
# push to logentries
# TODO: push to athena instead
le_handler.emit_raw(dat)
# then we publish them
msg = messaging.new_message()
msg.logMessage = dat
pub_sock.send(msg.to_bytes())
def __init__(self):
#ALCA params
self.ALCA_error = False
self.ALCA_lane_width = 3.6
self.ALCA_direction = 100 #none 0, left 1, right -1,reset 100
self.ALCA_step = 0
self.ALCA_total_steps = 20 * ALCA_duration_seconds #20 Hz, 5 seconds, wifey mode
self.ALCA_cancelling = False
self.ALCA_enabled = False
self.ALCA_OFFSET_C3 = 0.
self.ALCA_OFFSET_C2 = 0.
self.ALCA_OFFSET_C1 = 0.
self.ALCA_over_line = False
self.prev_CS_ALCA_error = False
self.ALCA_use_visual = True
self.ALCA_vego = 0.
self.ALCA_vego_prev = 0.
self.alcaStatus = messaging.sub_sock('alcaStatus', conflate=True)
self.alcaState = messaging.pub_sock('alcaState')
self.alcas = None
self.hit_prob_low = False
self.hit_prob_high = False
self.distance_to_line_L = 100.
self.prev_distance_to_line_L = 100.
self.distance_to_line_R = 100.
self.prev_distance_to_line_R = 100.
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context()
live100 = messaging.sub_sock(context,
service_list['live100'].port,
addr=addr,
conflate=True)
orbfeatures = messaging.sub_sock(context,
service_list['orbFeatures'].port,
addr=addr,
conflate=True)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
of = messaging.recv_one(orbfeatures)
l100 = messaging.recv_one_or_none(live100)
new_vp = calibrator.handle_orb_features(of)
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
if l100 is not None:
calibrator.handle_live100(l100)
if DEBUG:
calibrator.handle_debug()
calibrator.send_data(livecalibration)
def boardd_proxy_loop(rate=100, address="192.168.2.251"):
rk = Ratekeeper(rate)
can_init()
# *** subscribes can
logcan = messaging.sub_sock('can', addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def __init__(self, CP, CarController):
self.CP = CP
self.frame = 0
self.last_enable_pressed = 0
self.last_enable_sent = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.alca = messaging.pub_sock(service_list['alcaStatus'].port)
# *** init the major players ***
self.CS = CarState(CP)
self.VM = VehicleModel(CP)
mydbc = DBC[CP.carFingerprint]['pt']
if CP.carFingerprint == CAR.MODELS and self.CS.fix1916:
mydbc = mydbc + "1916"
self.cp = get_can_parser(CP, mydbc)
self.epas_cp = None
if self.CS.useWithoutHarness:
self.epas_cp = get_epas_parser(CP, 0)
else:
self.epas_cp = get_epas_parser(CP, 2)
self.pedal_cp = get_pedal_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name)
self.compute_gb = tesla_compute_gb
def __init__(self, CP):
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.angle_offset = 0.
self.init_angle_offset = False
self.acc_slow_on = False
self.pcm_acc_status = False
self.setspeedoffset = 34.0
self.Angles = np.zeros(250)
self.Angles_later = np.zeros(250)
self.Angle_counter = 0
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
if BASEDIR == "/data/openpilot" or BASEDIR == "/data/openpilot.arne182":
self.traffic_data_sock = messaging.pub_sock(
service_list['liveTrafficData'].port)
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def boardd_proxy_loop(rate=200, address="192.168.2.251"):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** subscribes can
logcan = messaging.sub_sock(context, service_list['can'].port, addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print "R:",hex(m[0]), str(m[2]).encode("hex")
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat.to_bytes())
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print "S:",hex(m[0]), str(m[2]).encode("hex")
can_send_many(cl)
rk.keep_time()
def __init__(self, mocked, RI):
self.current_time = 0
self.mocked = mocked
self.RI = RI
self.tracks = defaultdict(dict)
self.last_md_ts = 0
self.last_controls_state_ts = 0
self.active = 0
# v_ego
self.v_ego = 0.
self.v_ego_hist_t = deque([0], maxlen=v_len)
self.v_ego_hist_v = deque([0], maxlen=v_len)
self.v_ego_t_aligned = 0.
self.ready = False
self.icCarLR = None
if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE
) and CarSettings().get_value("useTeslaRadar"):
self.icCarLR = messaging.pub_sock(service_list['uiIcCarLR'].port)
self.lane_width = 3.0
#only used for left and right lanes
self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max
self.poller = zmq.Poller()
self.pathPlanSocket = messaging.sub_sock(service_list['pathPlan'].port,
conflate=True,
poller=self.poller)
self.dPoly = [0., 0., 0., 0.]
def boardd_mock_loop():
can_init()
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
logcan = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = list(filter(lambda x: x[-1] <= 2, snd))
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd)))
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd)))
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd)))
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
got_0 = len(list(filter(lambda x: x[-1] == 0 + 0x80, can_msgs)))
got_1 = len(list(filter(lambda x: x[-1] == 1 + 0x80, can_msgs)))
got_2 = len(list(filter(lambda x: x[-1] == 2 + 0x80, can_msgs)))
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" %
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1,
got_2))
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
sendcan.send(m.to_bytes())
def manager_thread():
# now loop
context = zmq.Context()
thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
health_sock = messaging.sub_sock(context, service_list['health'].port)
cloudlog.info("manager start")
start_managed_process("logmessaged")
start_managed_process("logcatd")
start_managed_process("uploader")
start_managed_process("ui")
# *** wait for the board ***
wait_for_device()
# flash the device
if os.getenv("NOPROG") is None:
boarddir = os.path.dirname(os.path.abspath(__file__)) + "/../board/"
os.system("cd %s && make" % boarddir)
start_managed_process("boardd")
if os.getenv("STARTALL") is not None:
for p in car_started_processes:
start_managed_process(p)
while 1:
# get health of board, log this in "thermal"
td = messaging.recv_sock(health_sock, wait=True)
print td
# replace thermald
msg = read_thermal()
thermal_sock.send(msg.to_bytes())
print msg
# TODO: add car battery voltage check
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2,
msg.thermal.cpu3) / 10.0
# uploader is gated based on the phone temperature
if max_temp > 85.0:
cloudlog.info("over temp: %r", max_temp)
kill_managed_process("uploader")
elif max_temp < 70.0:
start_managed_process("uploader")
# start constellation of processes when the car starts
if td.health.started:
for p in car_started_processes:
start_managed_process(p)
else:
for p in car_started_processes:
kill_managed_process(p)
# check the status of all processes, did any of them die?
for p in running:
cloudlog.info(" running %s %s" % (p, running[p]))
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame % rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def run_route(route):
can = messaging.pub_sock(service_list['can'].port)
CP = CarInterface.get_params(CAR.CIVIC, {})
signals, checks = get_can_signals(CP)
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1,
tcp_addr="127.0.0.1")
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1,
tcp_addr="127.0.0.1")
parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1)
if dict_keys_differ(parser_old.vl, parser_new.vl):
return False
lr = LogReader(route + ".bz2")
route_ok = True
t = 0
for msg in lr:
if msg.which() == 'can':
t += DT
msg_bytes = msg.as_builder().to_bytes()
can.send(msg_bytes)
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
updated_string = parser_string.update_string(t, msg_bytes)
if updated_old != updated_new:
route_ok = False
print(t, "Diff in seen")
if updated_new != updated_string:
route_ok = False
print(t, "Diff in seen string")
if dicts_vals_differ(parser_old.vl, parser_new.vl):
print(t, "Diff in dict")
route_ok = False
if dicts_vals_differ(parser_new.vl, parser_string.vl):
print(t, "Diff in dict string")
route_ok = False
return route_ok
