diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 93ec38f80bb372..6a34270ed54626 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -421,6 +421,14 @@ void *can_send_thread(void *crap) { zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); zmq_connect(subscriber, "tcp://127.0.0.1:8017"); + // drain sendcan to delete any stale messages from previous runs + zmq_msg_t msg; + zmq_msg_init(&msg); + int err = 0; + while(err >= 0) { + err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT); + } + // run as fast as messages come in while (!do_exit) { can_send(subscriber); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index c81060775b53fa..ef1c200aea0d0d 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -1,4 +1,8 @@ #!/usr/bin/env python + +# This file is not used by OpenPilot. Only boardd.cc is used. +# The python version is slower, but has more options for development. + import os import struct import zmq @@ -161,6 +165,9 @@ def boardd_loop(rate=200): # *** 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: @@ -203,6 +210,9 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"): # *** publishes to can send sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + # drain sendcan to delete any stale messages from previous runs + messaging.drain_sock(sendcan) + while 1: # recv @ 100hz can_msgs = can_recv() diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 4a82d9a022f36f..7bb4ef26f76a86 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -177,6 +177,14 @@ class CANParser { zmq_connect(subscriber, tcp_addr_char); + // drain sendcan to delete any stale messages from previous runs + zmq_msg_t msgDrain; + zmq_msg_init(&msgDrain); + int err = 0; + while(err >= 0) { + err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT); + } + dbc = dbc_lookup(dbc_name); assert(dbc); diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index e76d5ffa115b27..145cc5651fa52a 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,3 +1,4 @@ +import logging from common.numpy_fast import clip, interp from selfdrive.boardd.boardd import can_list_to_can_capnp # from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ @@ -54,6 +55,7 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ self.last_angle = 0 self.send_new_status = False # indicates we want to send 2a6 when we can. self.prev_2a6 = -9999 # long time ago. + self.prev_frame = -1 # previous frame from interface from 220 frame self.ccframe = 0 self.accel_steady = 0. self.car_fingerprint = car_fingerprint @@ -72,6 +74,10 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ self.packer = CANPacker(dbc_name) + logging.basicConfig(level=logging.DEBUG, filename="/tmp/chrylog", filemode="a+", + format="%(asctime)-15s %(levelname)-8s %(message)s") + logging.info('CarController init') + def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert): @@ -142,6 +148,10 @@ def update(self, sendcan, enabled, CS, frame, actuators, self.last_accel = apply_accel self.last_standstill = CS.standstill + if self.prev_frame == frame: + logging.info('prev_frame == frame so skipping') + return # Do not reuse an old frame. This avoids repeating on shut-down. + can_sends = [] #*** control msgs *** @@ -167,12 +177,14 @@ def update(self, sendcan, enabled, CS, frame, actuators, self.send_new_status = False self.prev_2a6 = self.ccframe new_msg = create_292(int(apply_steer * CAR_UNITS_PER_DEGREE), frame, moving_fast) + self.prev_frame = frame # save so we do not reuse frames sendcan.send(can_list_to_can_capnp([new_msg], msgtype='sendcan').to_bytes()) can_sends.append(new_msg) # degrees * 5.1 -> car steering units for msg in can_sends: [addr, _, dat, _] = msg - #outp = ('make_can_msg:%s len:%d %s' % ('0x{:02x}'.format(addr), len(dat), - # ' '.join('{:02x}'.format(ord(c)) for c in dat))) + outp = ('make_can_msg:%s len:%d %s' % ('0x{:02x}'.format(addr), len(dat), + ' '.join('{:02x}'.format(ord(c)) for c in dat))) + logging.info(outp) self.ccframe += 1 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 250f011b35f845..7752a266cf12be 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -1,3 +1,4 @@ +import logging from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.chrysler.values import DBC @@ -40,10 +41,19 @@ def get_can_parser(CP): ("ACC_STATUS_2", "ACC_2", 0), ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), - ("INCREMENTING_220", "LKAS_INDICATOR_1", 0), + ("INCREMENTING_220", "LKAS_INDICATOR_1", -1), ] + # It's considered invalid if it is not received for 10x the expected period (1/f). checks = [ + # sig_address, frequency + # ("GEAR", 50), + ("BRAKE_2", 50), + ("LKAS_INDICATOR_1", 100), + ("SPEED_1", 100), + ("WHEEL_SPEEDS", 50), + ("STEERING", 100), + ("ACC_2", 50), ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @@ -68,6 +78,10 @@ def __init__(self, CP): C=np.matrix([1.0, 0.0]), K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0.0 + logging.basicConfig(level=logging.DEBUG, filename="/tmp/chrylog-cs", filemode="a+", + format="%(asctime)-15s %(levelname)-8s %(message)s") + logging.info('CarState init') + def update(self, cp): # copy can_valid @@ -77,7 +91,8 @@ def update(self, cp): self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on - self.frame_220 = cp.vl["LKAS_INDICATOR_1"]['INCREMENTING_220'] + self.frame_220 = int(cp.vl["LKAS_INDICATOR_1"]['INCREMENTING_220']) + logging.info('frame_220 %d' % self.frame_220) self.door_all_closed = not any([cp.vl["DOORS"]['DOOR_OPEN_FL'], cp.vl["DOORS"]['DOOR_OPEN_FR'], diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 30ae411f5da43f..7f9af7d240a65a 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -271,17 +271,21 @@ def apply(self, c, perception_state=log.Live20Data.new_message()): # this wasn't needed in the game, so could it be needed for OP?!?! # check if it's more than 3 away, accounting for 0x10 wrap-around. - f1 = int(self.frame) % 0x10 - f2 = int(self.CS.frame_220) % 0x10 # shouldn't need the mod, but just in case. - fmin = min(f1, f2) - fmax = max(f1, f2) - if ((fmax - fmin) > 2) and ((fmin + 0x10 - fmax) > 2): - # copy lower nibble from frame_220 to our frame so they match - self.frame = (int(self.frame) & 0xfffffff0) | f2 + if (self.CS.frame_220 == -1): + return False # if we haven't seen a frame 220, then do not update. + self.frame = self.CS.frame_220 + + # f1 = int(self.frame) % 0x10 + # f2 = int(self.CS.frame_220) % 0x10 # shouldn't need the mod, but just in case. + # fmin = min(f1, f2) + # fmax = max(f1, f2) + # if ((fmax - fmin) > 2) and ((fmin + 0x10 - fmax) > 2): + # # copy lower nibble from frame_220 to our frame so they match + # self.frame = (int(self.frame) & 0xfffffff0) | f2 self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert) - self.frame += 1 + # self.frame += 1 return False