def create_sub_sock(ip, timeout):
os.environ["ZMQ"] = "1"
my_content = messaging_pyx.Context()
sync_sock = messaging_pyx.SubSocket()
addr = ip.encode('utf8')
sync_sock.connect(my_content, 'testLiveLocation', addr, conflate=True)
sync_sock.setTimeout(timeout)
del os.environ["ZMQ"]
return sync_sock, my_content
def msg_sync_thread():
#start_sec = round(time.time())
sync_topics = [
'modelV2', 'carState', 'liveTracks', 'radarState', 'controlsState'
]
# dMonitoringState', 'gpsLocation', 'radarState', 'model', 'gpsLocationExternal',
# 'pathPlan', 'liveCalibration', laneSpeed
frame_counts = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
SLEEP_TIME = 0.01
frame_limits = [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]
sub_list = []
pub_list = []
for topic in sync_topics:
sub_list.append(messaging.sub_sock(topic, conflate=True, timeout=100))
os.environ["ZMQ"] = "1"
pub_context = messaging_pyx.Context()
for topic in sync_topics:
pub = messaging_pyx.PubSocket()
pub.connect(pub_context, topic)
pub_list.append(pub)
del os.environ["ZMQ"]
while True:
for index, sub in enumerate(sub_list):
try:
data = sub.receive()
if data:
#print ('sending data ' + sync_topics[index])
frame_limit = frame_limits[index]
if frame_limit != 0 and (frame_counts[index] % frame_limit
!= 0):
pass
else:
pub_list[index].send(data)
frame_counts[index] += 1
except messaging_pyx.MessagingError:
print('msg_sync MessagingError error happens ' +
sync_topics[index])
time.sleep(SLEEP_TIME)
def can_sync_thread():
can_sock = messaging.sub_sock('can', conflate=True, timeout=100)
can_pub = None
os.environ["ZMQ"] = "1"
pub_context = messaging_pyx.Context()
can_pub = messaging_pyx.PubSocket()
can_pub.connect(pub_context, 'testJoystick')
del os.environ["ZMQ"]
rk = Ratekeeper(100.0, print_delay_threshold=None)
cc_main = 0
cc_status = 0
speed = 0.0
dbc_file = "toyota_yaris"
signals = [
# sig_name, sig_address, default
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE2", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 0),
("DOOR_OPEN_FL", "SEATS_DOORS", 0),
("DOOR_OPEN_FR", "SEATS_DOORS", 0),
("DOOR_OPEN_RL", "SEATS_DOORS", 0),
("DOOR_OPEN_RR", "SEATS_DOORS", 0),
("MAIN_ON", "PCM_CRUISE_SM", 0),
("CRUISE_CONTROL_STATE", "PCM_CRUISE_SM", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 0), # 3 is no blinkers
("ENGINE_RPM", "POWERTRAIN", 0),
("SPEED", "SPEED", 0),
("MAY_CONTAIN_LIGHTS", "BOOLS", 0),
("CHANGES_EACH_RIDE", "SLOW_VARIABLE_INFOS", 0),
("INCREASING_VALUE_FUEL", "SLOW_VARIABLE_INFOS", 0)
]
checks = []
parser = CANParser(dbc_file, signals, checks, 0)
play_time = 0
while True:
try:
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
parser.update_strings(can_strs)
# print (parser.vl)
cc_main = parser.vl['PCM_CRUISE_SM']['MAIN_ON']
cc_status = parser.vl['PCM_CRUISE_SM']['CRUISE_CONTROL_STATE']
speed = parser.vl['SPEED']['SPEED']
doorOpen = any([
parser.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
seatbeltUnlatched = parser.vl["SEATS_DOORS"][
'SEATBELT_DRIVER_UNLATCHED'] != 0
light = parser.vl["BOOLS"]["MAY_CONTAIN_LIGHTS"]
a = parser.vl["SLOW_VARIABLE_INFOS"]["CHANGES_EACH_RIDE"]
b = parser.vl["SLOW_VARIABLE_INFOS"]["INCREASING_VALUE_FUEL"]
if doorOpen:
play_time = play_sound(SOUND_PATH + 'door_open.wav', play_time,
3)
if seatbeltUnlatched:
play_time = play_sound(SOUND_PATH + 'seatbelt.wav', play_time,
3)
if light != 0:
play_time = play_sound(SOUND_PATH + 'turn_signal.wav',
play_time, 3)
cc_main_v = 0
if cc_main:
cc_main_v = 1
can_dict = {
'cc_main': cc_main_v,
'cc_status': cc_status,
'speed': speed,
'light': light,
'a': a,
'b': b
}
json_str = json.dumps(can_dict)
json_str = json_str.replace('\'', '"')
can_pub.send(json_str)
except messaging_pyx.MessagingError:
print('MessagingError error happens')
rk.keep_time()
def main():
global last_debug_mode
global pm
global op_params
print(
'************************************************** phone_control start **************************************************'
)
os.system(
'cp /data/openpilot/continue.sh /data/data/com.termux/files/; sync')
os.system('cp /data/openpilot/op_params.json /data/; sync')
op_params = opParams()
clear_params(op_params)
ip = try_to_connect()
last_ip = None
sync_sock = None
os.environ["ZMQ"] = "1"
sync_content = messaging_pyx.Context()
del os.environ["ZMQ"]
if ip:
sync_sock = create_sub_sock(ip, sync_content, timeout=TIME_OUT)
last_ip = ip
rk = Ratekeeper(10.0, print_delay_threshold=None)
pm = messaging.PubMaster(['liveMapData'])
last_debug_mode = 0
no_data_received_num = 0
LOST_CONNECTION_NUM = 20
git_fetched = False
start_sec = sec_since_boot()
while 1:
sync_data = None
# if ip is not connected, try to reconnect
if not ip:
time.sleep(1)
ip = try_to_connect(last_ip)
if ip:
sync_sock = create_sub_sock(ip, sync_content, timeout=TIME_OUT)
last_ip = ip
no_data_received_num = 0
if sync_sock:
sync_data = sync_sock.receive_golden()
# print ('sync_data=' + str(sync_data))
if not sync_data:
no_data_received_num += 1
if no_data_received_num >= LOST_CONNECTION_NUM:
# print ('lost connection of ' + str(ip))
sync_sock = None
last_ip = ip
ip = None
no_data_received_num = 0
else:
no_data_received_num = 0
if sync_data:
process_phone_data(sync_data)
# simple OTA instead of updated
# we will just do a git pull once we connected to WIFI
# and mark a flag file /tmp/op_git_updated
# then controlsd will send out alert accordingly
if not git_fetched:
cur_sec = sec_since_boot()
if (cur_sec - start_sec) >= 10:
if is_on_wifi():
print(
'*************************************** try to git fetch ***************************************'
)
git_fetched = True
cur_git_hash = get_git_hash()
os.system("cd /data/openpilot; git pull;")
next_git_hash = get_git_hash()
if next_git_hash != cur_git_hash:
os.system('echo 1 > /tmp/op_git_updated')
else:
start_sec = sec_since_boot()
#sm.update()
rk.keep_time()