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 __init__(self):
# socks
self.location = messaging.sub_sock(service_list['gpsLocation'].port)
self.health = messaging.sub_sock(service_list['health'].port)
self.thermal = messaging.sub_sock(service_list['thermal'].port)
# time management
self.fast_mode = False
self.last_read = 0
self.last_send = 0
if self.fast_mode:
self.time_to_read = 5
self.time_to_send = 6
else:
self.time_to_read = 59
self.time_to_send = 60
# gpsLocation
self.latitude = -1
self.longitude = -1
self.altitude = -1
self.speed = -1
# health
self.car_voltage = -1
# thermal
self.eon_soc = -1
self.bat_temp = -1
self.usbonline = None
self.started = None
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):
self.CP = CP
self.poller = zmq.Poller()
self.arne182Status = messaging.sub_sock(
service_list['arne182Status'].port,
poller=self.poller,
conflate=True)
self.latcontolStatus = messaging.sub_sock(
service_list['latControl'].port, poller=self.poller, conflate=True)
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
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.path_x = np.arange(192)
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 __init__(self,
lead_relevancy=False,
rate=100,
speed=0.0,
distance_lead=2.0):
self.rate = rate
if not Plant.messaging_initialized:
Plant.logcan = messaging.pub_sock(service_list['can'].port)
Plant.sendcan = messaging.sub_sock(service_list['sendcan'].port)
Plant.model = messaging.pub_sock(service_list['model'].port)
Plant.live_params = messaging.pub_sock(
service_list['liveParameters'].port)
Plant.health = messaging.pub_sock(service_list['health'].port)
Plant.thermal = messaging.pub_sock(service_list['thermal'].port)
Plant.driverMonitoring = messaging.pub_sock(
service_list['driverMonitoring'].port)
Plant.cal = messaging.pub_sock(
service_list['liveCalibration'].port)
Plant.controls_state = messaging.sub_sock(
service_list['controlsState'].port)
Plant.plan = messaging.sub_sock(service_list['plan'].port)
Plant.messaging_initialized = True
self.frame = 0
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()
self.response_seen = False
time.sleep(1)
messaging.drain_sock(Plant.sendcan)
messaging.drain_sock(Plant.controls_state)
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.automated_cruise_action_time = 0
self.last_angle = 0.
context = zmq.Context()
self.poller = zmq.Poller()
self.live20 = messaging.sub_sock(context,
service_list['live20'].port,
conflate=True,
poller=self.poller)
self.live_map_data = messaging.sub_sock(
context,
service_list['liveMapData'].port,
conflate=True,
poller=self.poller)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.
self.pedal_idx = 0
self.pedal_steady = 0.
self.prev_tesla_accel = 0.
self.prev_tesla_pedal = 0.
self.pedal_interceptor_state = 0
self.torqueLevel_last = 0.
self.prev_v_ego = 0.
self.PedalForZeroTorque = 18. #starting number, works on my S85
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.
self.a_pid = 0.
self.last_output_gb = 0.
self.last_speed_kph = 0.
#for smoothing the changes in speed
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
#Long Control
self.LoC = None
#when was radar data last updated?
self.last_md_ts = None
self.last_l100_ts = None
self.md_ts = None
self.l100_ts = None
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
self.params = Params()
def plannerd_thread(gctx):
context = zmq.Context()
poller = zmq.Poller()
carstate = messaging.sub_sock(context, service_list['carState'].port, poller)
live20 = messaging.sub_sock(context, service_list['live20'].port)
model = messaging.sub_sock(context, service_list['model'].port)
plan = messaging.pub_sock(context, service_list['plan'].port)
# wait for stats about the car to come in from controls
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams")
CS = None
PP = PathPlanner(model)
AC = AdaptiveCruise(live20)
# start the loop
set_realtime_priority(2)
# this runs whenever we get a packet that can change the plan
while True:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN or sock != carstate:
continue
cur_time = sec_since_boot()
CS = messaging.recv_sock(carstate).carState
PP.update(cur_time, CS.vEgo)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP)
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# lateral plan
plan_send.plan.lateralValid = not PP.dead
if plan_send.plan.lateralValid:
plan_send.plan.dPoly = map(float, PP.d_poly)
# longitudal plan
plan_send.plan.longitudinalValid = not AC.dead
if plan_send.plan.longitudinalValid:
plan_send.plan.vTarget = float(AC.v_target_lead)
plan_send.plan.aTargetMin = float(AC.a_target[0])
plan_send.plan.aTargetMax = float(AC.a_target[1])
plan_send.plan.jerkFactor = float(AC.jerk_factor)
plan_send.plan.hasLead = AC.has_lead
plan.send(plan_send.to_bytes())
def main(gctx=None):
logger = Logger(ROOT, gen_init_data(gctx))
context = zmq.Context()
poller = zmq.Poller()
# we push messages to visiond to rotate image recordings
vision_control_sock = context.socket(zmq.PUSH)
vision_control_sock.connect("tcp://127.0.0.1:8001")
# register listeners for all services
for service in service_list.itervalues():
if service.should_log and service.port is not None:
messaging.sub_sock(context, service.port, poller)
uploader.clear_locks(ROOT)
cur_dir, cur_part = logger.start()
try:
cloudlog.info("starting in dir %r", cur_dir)
rotate_msg = messaging.log.LogRotate.new_message()
rotate_msg.segmentNum = cur_part
rotate_msg.path = cur_dir
vision_control_sock.send(rotate_msg.to_bytes())
last_rotate = realtime.sec_since_boot()
while True:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN:
continue
dat = sock.recv()
# print "got", len(dat), realtime.sec_since_boot()
# logevent = log_capnp.Event.from_bytes(dat)
# print str(logevent)
logger.log_data(dat)
t = realtime.sec_since_boot()
if (t - last_rotate) > SEGMENT_LENGTH:
last_rotate += SEGMENT_LENGTH
cur_dir, cur_part = logger.rotate()
cloudlog.info("rotated to %r", cur_dir)
rotate_msg = messaging.log.LogRotate.new_message()
rotate_msg.segmentNum = cur_part
rotate_msg.path = cur_dir
vision_control_sock.send(rotate_msg.to_bytes())
finally:
logger.stop()
def calibrationd_thread(gctx):
context = zmq.Context()
features = messaging.sub_sock(context, service_list['features'].port)
live100 = messaging.sub_sock(context, service_list['live100'].port)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
# subscribe to stats about the car
VP = VehicleParams(False)
v_ego = None
calib = load_calibration(gctx)
last_cal_cycle = calib.cal_cycle
while 1:
# calibration at the end so it does not delay radar processing above
ft = messaging.recv_sock(features, wait=True)
# get latest here
l100 = messaging.recv_sock(live100)
if l100 is not None:
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
if v_ego is None:
continue
p0 = ft.features.p0
p1 = ft.features.p1
st = ft.features.status
calib.calibration(p0, p1, st, v_ego, steer_angle, VP)
# write a new calibration every 100 cal cycle
if calib.cal_cycle - last_cal_cycle >= 100:
print "writing cal", calib.cal_cycle
with open(CALIBRATION_FILE, "w") as cal_file:
cal_file.write(str(calib.cal_cycle) + '\n')
cal_file.write(str(calib.cal_status) + '\n')
cal_file.write(str(calib.vp_f[0]) + '\n')
cal_file.write(str(calib.vp_f[1]) + '\n')
last_cal_cycle = calib.cal_cycle
warp_matrix = map(float, calib.warp_matrix.reshape(9).tolist())
dat = messaging.new_message()
dat.init('liveCalibration')
dat.liveCalibration.warpMatrix = warp_matrix
dat.liveCalibration.calStatus = calib.cal_status
dat.liveCalibration.calCycle = calib.cal_cycle
dat.liveCalibration.calPerc = calib.cal_perc
livecalibration.send(dat.to_bytes())
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, CarController):
self.CP = CP
self.CC = CarController
cloudlog.debug("Using Mock Car Interface")
# TODO: subscribe to phone sensor
self.sensor = messaging.sub_sock('sensorEvents')
self.gps = messaging.sub_sock('gpsLocation')
self.speed = 0.
self.prev_speed = 0.
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.pcc_available = self.prev_pcc_available = False
self.pedal_timeout_frame = 0
self.accelerator_pedal_pressed = self.prev_accelerator_pedal_pressed = False
self.automated_cruise_action_time = 0
self.last_angle = 0.
self.radarState = messaging.sub_sock('radarState', conflate=True)
self.live_map_data = messaging.sub_sock('liveMapData', conflate=True)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.
self.speed_limit_kph = 0.
self.prev_speed_limit_kph = 0.
self.pedal_idx = 0
self.pedal_steady = 0.
self.prev_tesla_accel = 0.
self.prev_tesla_pedal = 0.
self.torqueLevel_last = 0.
self.prev_v_ego = 0.
self.PedalForZeroTorque = 18. #starting number, works on my S85
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.
self.a_pid = 0.
self.last_output_gb = 0.
self.last_speed_kph = None
#for smoothing the changes in speed
self.v_acc_start = 0.0
self.a_acc_start = 0.0
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
#Long Control
self.LoC = None
#when was radar data last updated?
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
self.params = Params()
average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second)
self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
def __init__(self, CP, sendcan=None):
self.CP = CP
print "Using Mock Car Interface"
context = zmq.Context()
# TODO: subscribe to phone sensor
self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port)
self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
self.speed = 0.
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
def __init__(self, carcontroller):
self.time_last_car_detected = 0
self.time_last_high_beam_cancel = 0
self.prev_car_present = False
self.prev_highLowBeamStatus = 0
self.prev_highLowBeamReason = 0
self.prev_light_stalk_position = 0
self.prev_high_beam_on = False
self.prev_lights_on = False
self.ahbInfo = messaging.sub_sock('ahbInfo', conflate=True)
self.ahbInfoData = None
self.ahbIsEnabled = False
self.frameInfo = messaging.sub_sock('frame', conflate=True)
self.frameInfoData = None
self.frameInfoGain = 0
def getMessage(service=None, timeout=1000):
if service is None or service not in service_list:
raise Exception("invalid service")
socket = messaging.sub_sock(service_list[service].port)
socket.setsockopt(zmq.RCVTIMEO, timeout)
ret = messaging.recv_one(socket)
return ret.to_dict()
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 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 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 manager_thread():
# now loop
thermal_sock = messaging.sub_sock(service_list['thermal'].port)
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
# start daemon processes
for p in daemon_processes:
start_daemon_process(p, params)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start frame
pm_apply_packages('enable')
system("am start -n ai.comma.plus.frame/.MainActivity")
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
logger_dead = False
while 1:
# get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
# uploader is gated based on the phone temperature
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
kill_managed_process("uploader")
else:
start_managed_process("uploader")
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
for p in car_started_processes:
kill_managed_process(p)
# check the status of all processes, did any of them die?
running_list = [" running %s %s" % (p, running[p]) for p in running]
cloudlog.debug('\n'.join(running_list))
# is this still needed?
if params.get("DoUninstall") == "1":
break
def test_loop():
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
CI, CP = get_car(logcan)
state = 0
states = [
"'seatbeltNotLatched' in CS.errors", "CS.gasPressed",
"CS.brakePressed", "CS.steeringPressed", "bpressed(CS, 'leftBlinker')",
"bpressed(CS, 'rightBlinker')", "bpressed(CS, 'cancel')",
"bpressed(CS, 'accelCruise')", "bpressed(CS, 'decelCruise')",
"bpressed(CS, 'altButton1')", "'doorOpen' in CS.errors", "False"
]
while 1:
CC = car.CarControl.new_message()
# read CAN
CS = CI.update(CC)
while eval(states[state]) == True:
state += 1
print "IN STATE %d: waiting for %s" % (state, states[state])
def __init__(self, dbc_name, car_fingerprint, enable_camera):
self.apply_steer_last = 0
self.turning_signal_timer = 0
self.car_fingerprint = car_fingerprint
self.lkas11_cnt = 0
self.mdps12_cnt = 0
self.cnt = 0
self.last_resume_cnt = 0
self.map_speed = 0
self.enable_camera = enable_camera
# True when camera present, and we need to replace all the camera messages
# otherwise we forward the camera msgs and we just replace the lkas cmd signals
self.camera_disconnected = False
self.packer = CANPacker(dbc_name)
context = zmq.Context()
self.params = Params()
self.map_data_sock = messaging.sub_sock(
context, service_list['liveMapData'].port, conflate=True)
self.speed_conv = 3.6
self.speed_adjusted = False
self.checksum = "NONE"
self.checksum_learn_cnt = 0
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.lkas = False
self.spas_present = True # TODO Make Automatic
self.ALCA = ALCAController(
self, True, False) # Enabled True and SteerByAngle only False
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 can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
logcan = messaging.sub_sock(service_list['can'].port, addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", bus))
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
if sec_since_boot() - lp > 0.1:
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k, v in sorted(
zip(msgs.keys(),
map(lambda x: binascii.hexlify(x[-1]),
msgs.values()))):
if max_msg is None or k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k, k), len(
msgs[k]), v.decode('ascii'))
print(dd)
lp = sec_since_boot()
def __init__(self, dbc_name, car_fingerprint):
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
self.lkas11_cnt = 0
self.clu11_cnt = 0
self.mdps12_cnt = 0
self.cnt = 0
self.last_resume_cnt = 0
self.map_speed = 0
self.map_data_sock = messaging.sub_sock(service_list['liveMapData'].port)
self.params = Params()
self.speed_conv = 3.6
self.speed_offset = 1.03 # Multiplier for cruise speed vs speed limit TODO: Add to UI
self.speed_enable = True # Enable Auto Speed Set TODO: Add to UI
self.speed_adjusted = False
self.checksum = "NONE"
self.checksum_learn_cnt = 0
self.turning_signal_timer = 0
self.camera_disconnected = False
self.checksum_found = False
self.packer = CANPacker(dbc_name)
def main(gctx=None):
poller = zmq.Poller()
sock = messaging.sub_sock(service_list['controlsState'].port, poller)
poller.poll(timeout=1000)
last_v_ego = 0.
last_active = False
env = dict(os.environ)
env['LD_LIBRARY_PATH'] = mediaplayer
while 1:
v_ego = 0
active = False
controls_state = messaging.recv_sock(sock, wait=True)
if controls_state is not None:
v_ego = controls_state.controlsState.vEgo
active = controls_state.controlsState.active
# we are driving and all of sudden we dont have any speed at all
# we better warn the driver before it's too late
if last_active and last_v_ego >= 5 and v_ego == 0:
subprocess.Popen([mediaplayer + 'mediaplayer', '/data/openpilot/selfdrive/dragonpilot/safeguardd/error.wav'], shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True)
last_active = active
last_v_ego = v_ego
time.sleep(0.1)
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):
#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 can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"):
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", bus))
f = open("can1_dsu_off_frc_off.txt", "w")
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
if sec_since_boot() - lp > 0.1:
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k, v in sorted(
zip(msgs.keys(),
map(lambda x: x[-1].encode("hex"), msgs.values()))):
if k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" %
(k, k), len(msgs[k]), v)
print dd
f.write(dd)
lp = sec_since_boot()
f.close()
def __init__(self):
self.poller = zmq.Poller()
self.sensorEvents = messaging.sub_sock(
service_list['sensorEvents'].port,
conflate=True,
poller=self.poller)
self.roll = 0.
self.pitch = 0.01
self.yaw = 9.8
self.roll_list = deque([0., 0., 0., 0., 0.])
self.pitch_list = deque([0., 0., 0., 0., 0.])
self.yaw_list = deque([0., 0., 0., 0., 0.])
self.mag_roll = 0.
self.mag_pitch = 0.01
self.mag_yaw = 9.8
self.mag_roll_list = deque([0., 0., 0., 0., 0.])
self.mag_pitch_list = deque([0., 0., 0., 0., 0.])
self.mag_yaw_list = deque([0., 0., 0., 0., 0.])
self.gyro_roll = 0.
self.gyro_pitch = 0.01
self.gyro_yaw = 9.8
self.gyro_roll_list = deque([0., 0., 0., 0., 0.])
self.gyro_pitch_list = deque([0., 0., 0., 0., 0.])
self.gyro_yaw_list = deque([0., 0., 0., 0., 0.])
self.gyro_t_ns = 0
self.last_gyro_t_ns = 0
