From 75d986fc0434b1fb80373eb0a7fefbb07e5380e0 Mon Sep 17 00:00:00 2001 From: BogGyver <39277124+BogGyver@users.noreply.github.com> Date: Wed, 4 Sep 2019 09:21:22 -0400 Subject: [PATCH] Promote panda fixes from alpha to devel (#98) * Fix for mock car crash * fix ALCA module * do not use * radar_interface logic improvements * remove radar logging * radar_interface refactoring * Log radar_interface CAN error, Process Comm Errors (#95) * Log radar_interface CAN error * Add support for process comm error logging * Improve Process Comm error logging * Can and CommError throttling * Update radar_interface.py * fixing push per Raf's comments * ALCA and radar_interface work (#96) (#97) * do not use * radar_interface logic improvements * remove radar logging * radar_interface refactoring * Update radar_interface.py * fixing push per Raf's comments * fix process comm issue * switch radard from lane_parser to model * radar/ALCA changes * fix lane width * switched back to not deleting CarParams to avoid radar failure when rebooting EON while driving * cleanup some debug info to be able to restart ALCA research * tweaks on safety_tesla.h for can bus isolation * few more cleanups to safety_tesla.h * fix start signal logic --- cereal/tesla.capnp | 1 + common/params.py | 2 +- panda/board/safety/safety_tesla.h | 34 +++--- selfdrive/can/plant_can_parser.py | 6 -- selfdrive/car/modules/ALCA_module.py | 127 ++++++++++------------- selfdrive/car/tesla/carcontroller.py | 3 + selfdrive/car/tesla/interface.py | 2 +- selfdrive/car/tesla/radar_interface.py | 57 ++++++---- selfdrive/car/tesla/teslacan.py | 7 ++ selfdrive/controls/controlsd.py | 16 ++- selfdrive/controls/lib/lane_planner.py | 10 +- selfdrive/controls/lib/latcontrol_pid.py | 5 +- selfdrive/controls/lib/pathplanner.py | 16 +-- selfdrive/controls/radard.py | 41 ++++---- selfdrive/messaging.py | 57 +++++++++- selfdrive/tinklad/tinkla_interface.py | 62 ++++++++--- selfdrive/tinklad/tinkladTestClient.py | 25 +++-- 17 files changed, 290 insertions(+), 181 deletions(-) diff --git a/cereal/tesla.capnp b/cereal/tesla.capnp index 5f31f98ed..b81c4126d 100644 --- a/cereal/tesla.capnp +++ b/cereal/tesla.capnp @@ -35,6 +35,7 @@ struct TeslaRadarPoint { movingState @3 :UInt8; # 0-indeterminate 1-moving 2-stopped 3-standing length @4 :Float32; # length in meters obstacleProb @5 :Float32; # probability to be an obstacle + timeStamp @6 :UInt64; #timestamp when the pair was received } struct ICCarsLR { diff --git a/common/params.py b/common/params.py index 461e53817..fc02804ec 100755 --- a/common/params.py +++ b/common/params.py @@ -53,7 +53,7 @@ class UnknownKeyName(Exception): "AccessToken": [TxType.PERSISTENT], "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], - "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], #[TxType.CLEAR_ON_CAR_START], + "CarParams": [TxType.CLEAR_ON_CAR_START], #[TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], #[TxType.CLEAR_ON_CAR_START], "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], #[TxType.PERSISTENT], #[TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CompletedTrainingVersion": [TxType.PERSISTENT], "ControlsParams": [TxType.PERSISTENT], diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 02726bf35..35c937483 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -59,10 +59,11 @@ int high_beam_lever_state = 0; int tesla_radar_status = 0; //0-not present, 1-initializing, 2-active uint32_t tesla_last_radar_signal = 0; -const uint32_t TESLA_RADAR_TIMEOUT = 1000000; // 1 second between real time checks +const uint32_t TESLA_RADAR_TIMEOUT = 10000000; // 10s second between real time checks char radar_VIN[] = " "; //leave empty if your radar VIN matches the car VIN int tesla_radar_vin_complete = 0; int tesla_radar_can = 1; +int tesla_epas_can = 2; int tesla_radar_trigger_message_id = 0; //not used by tesla, to showcase for other cars int radarPosition = 0; //0 nosecone, 1 facelift int radarEpasType = 0; //0/1 bosch, 2-4 mando @@ -945,7 +946,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //let's see if the pedal was pressed - if ((addr == 0x552) && (bus_number == 2)) { + if ((addr == 0x552) && (bus_number == tesla_epas_can)) { //m1 = 0.050796813 //m2 = 0.101593626 //d = -22.85856576 @@ -953,7 +954,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //we use 0x108 at 100Hz to detect timing of messages sent by our fake DAS and EPB - if (addr == 0x108) { + if ((addr == 0x108) && (bus_number == 0)) { if (fake_DAS_counter % 10 == 5) { do_EPB_epasControl(to_push->RIR,to_push->RDTR); } @@ -962,8 +963,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } // Record the current car time in current_car_time (for use with double-pulling cruise stalk) - if (addr == 0x318) - { + if ((addr == 0x318) && (bus_number == 0)) { int hour = (to_push->RDLR & 0x1F000000) >> 24; int minute = (to_push->RDHR & 0x3F00) >> 8; int second = (to_push->RDLR & 0x3F0000) >> 16; @@ -971,7 +971,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //we use EPAS_sysStatus 0x370 to determine if the car is off or on - if (addr == 0x370) { + if ((addr == 0x370) && (bus_number != 1)) { time_last_EPAS_data = current_car_time; } @@ -986,7 +986,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //see if cruise is enabled [Enabled, standstill or Override] and cancel if using pedal - if (addr == 0x368) { + if ((addr == 0x368) && (bus_number == 0)) { //first save values for spamming DAS_diStateL = to_push->RDLR; DAS_diStateH = to_push->RDHR; @@ -999,7 +999,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //looking for radar messages; - if ((addr == 0x300) && (bus_number ==1)) + if ((addr == 0x300) && (bus_number == tesla_radar_can)) { uint32_t ts = TIM2->CNT; uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); @@ -1018,7 +1018,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //0x631 is sent by radar to initiate the sync - if ((addr == 0x631) && (bus_number == 1)) + if ((addr == 0x631) && (bus_number == tesla_radar_can)) { uint32_t ts = TIM2->CNT; uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); @@ -1036,8 +1036,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } } - if (addr == 0x45) - { + if ((addr == 0x45) && (bus_number == 0)) { //first save for future use DAS_lastStalkL = to_push->RDLR; DAS_lastStalkH = to_push->RDHR; @@ -1240,7 +1239,8 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) // exit controls on EPAS error // EPAS_sysStatus::EPAS_eacStatus 0x370 - if ((addr == 0x370) && (bus_number == 1)) + //BB this was on bus_number 1 which is wrong, but not tested on 2 yet + if ((addr == 0x370) && (bus_number == tesla_epas_can)) { // if EPAS_eacStatus is not 1 or 2, disable control eac_status = (GET_BYTE(to_push, 6) >> 5) & 0x7; @@ -1293,8 +1293,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) /* <-- revB giraffe GPIO */ //BO_ 1001 DAS_bodyControls: 8 XXX - if (addr == 0x3e9) - { + if ((addr == 0x3e9) && (bus_number == 0)) { int high_beam_decision = (to_push->RDLR >> 10) & 0x3; //DAS_highLowBeamDecision : 10|2@1+ // highLowBeamDecision: //0: Undecided (Car off) @@ -1320,8 +1319,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //DAS_bodyControls //BO_ 872 DI_state: 8 DI - if (addr == 0x368) - { + if ((addr == 0x368) && (bus_number == 0)) { int regen_brake_light = (to_push->RDLR >> 8) & 0x1; //DI_regenLight : 8|1@1+ //if the car's brake lights are on, set pin 2 to high if (regen_brake_light == 1) @@ -1863,7 +1861,7 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) return -1; } - if (bus_num == 2) + if (bus_num == tesla_epas_can) { // remove GTW_epasControl in forwards @@ -1893,4 +1891,4 @@ const safety_hooks tesla_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = tesla_ign_hook, .fwd = tesla_fwd_hook, -}; \ No newline at end of file +}; diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py index 40799db83..b2a3a96de 100644 --- a/selfdrive/can/plant_can_parser.py +++ b/selfdrive/can/plant_can_parser.py @@ -6,7 +6,6 @@ from selfdrive.car.honda.hondacan import fix from common.realtime import sec_since_boot from common.dbc import dbc -from selfdrive.tinklad.tinkla_interface import TinklaClient class CANParser(object): def __init__(self, dbc_f, signals, checks=None): @@ -57,8 +56,6 @@ def __init__(self, dbc_f, signals, checks=None): for i, x in enumerate(self._msgs): self._message_indices[x].append(i) - self.tinklaClient = TinklaClient() - def update_can(self, can_recv): msgs_upd = [] cn_vl_max = 5 # no more than 5 wrong counter checks @@ -82,7 +79,6 @@ def update_can(self, can_recv): # compare recalculated vs received checksum if msg_vl != cdat: print("CHECKSUM FAIL: {0}".format(hex(msg))) - self.tinklaClient.logCANErrorEvent(canMessage=msg, additionalInformation="Checksum failure") self.ck[msg] = False self.ok[msg] = False # counter check @@ -98,7 +94,6 @@ def update_can(self, can_recv): # message status is invalid if we received too many wrong counter values if self.cn_vl[msg] >= cn_vl_max: print("COUNTER WRONG: {0}".format(hex(msg))) - self.tinklaClient.logCANErrorEvent(canMessage=msg, additionalInformation="Too many wrong counter values") self.ok[msg] = False # update msg time stamps and counter value @@ -134,5 +129,4 @@ def _check_dead_msgs(self): for msg in set(self._msgs): if msg in self.msgs_ck and self.sec_since_boot_cached - self.ct[msg] > 10./self.frqs[msg]: self.ok[msg] = False - self.tinklaClient.logCANErrorEvent(canMessage=msg, additionalInformation="Dead message") diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 183545ea6..341d3dfbf 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -58,10 +58,11 @@ #ALCA ALCA_line_check_low_limit = 0.25 ALCA_line_check_high_limit = 0.75 -ALCA_line_min_prob = 0.1 +ALCA_line_min_prob = 0.01 ALCA_release_distance = 0.3 ALCA_DEBUG = True +DEBUG_INFO = "step {step} of {total_steps}: direction={ALCA_direction} | using visual = {ALCA_use_visual} | over line={ALCA_over_line} | lane width={ALCA_lane_width} | left to move={left_to_move} | from center ={from_center} | C2 offset = {ALCA_OFFSET_C2} | " class ALCAController(object): def __init__(self,carcontroller,alcaEnabled,steerByAngle): @@ -248,14 +249,11 @@ def __init__(self): self.ALCA_over_line = False self.prev_CS_ALCA_error = False self.ALCA_use_visual = True - self.ALCA_l_poly = np.array([0., 0., 0., -100.]) - self.ALCA_r_poly = np.array([0., 0., 0., -100.]) self.ALCA_vego = 0. self.ALCA_vego_prev = 0. - self.mx = 0. - self.dx = 0. self.poller = zmq.Poller() self.alcaStatus = messaging.sub_sock(service_list['alcaStatus'].port, conflate=True, poller=self.poller) + self.alcaState = messaging.pub_sock(service_list['alcaState'].port) self.alcas = None @@ -269,29 +267,36 @@ def reset_alca (self): self.ALCA_OFFSET_C2 = 0. self.ALCA_over_line = False self.ALCA_use_visual = True - self.ALCA_l_poly = np.array([0., 0., 0., -100.]) - self.ALCA_r_poly = np.array([0., 0., 0., -100.]) self.ALCA_vego_prev = 0. - self.mx = 0. - self.dx = 0. self.alcas = None def debug_alca(self,message): if ALCA_DEBUG: print message - def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width): - - for socket, _ in self.poller.poll(1): + def send_state(self): + alca_state = tesla.ALCAState.new_message() + #ALCA params + alca_state.alcaDirection = int(self.ALCA_direction) + alca_state.alcaError = bool(self.ALCA_error) + alca_state.alcaCancelling = bool(self.ALCA_cancelling) + alca_state.alcaEnabled = bool(self.ALCA_enabled) + alca_state.alcaLaneWidth = float(self.ALCA_lane_width) + alca_state.alcaStep = int(self.ALCA_step) + alca_state.alcaTotalSteps = int(self.ALCA_total_steps) + self.alcaState.send(alca_state.to_bytes()) + + def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width, p_poly): + + for socket, _ in self.poller.poll(0): if socket is self.alcaStatus: self.alcas = tesla.ALCAStatus.from_bytes(socket.recv()) - - if not self.ALCA_enabled: - return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width #if we don't have yet ALCA status, return same values if self.alcas is None: - return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width + self.send_state() + return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly + self.ALCA_direction = self.alcas.alcaDirection self.ALCA_enabled = self.alcas.alcaEnabled @@ -299,20 +304,14 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width): self.ALCA_error = self.ALCA_error or (self.alcas.alcaError and not self.prev_CS_ALCA_error) self.prev_CS_ALCA_error = self.alcas.alcaError + if not self.ALCA_enabled: + self.send_state() + return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly + #if error but no direction, the carcontroller component is fine and we need to reset if self.ALCA_error and (self.ALCA_direction == 0): self.ALCA_error = False - mx = 0. - if self.ALCA_enabled and not (self.ALCA_direction == 0): - mx = min(abs(r_poly[3] - self.ALCA_r_poly[3]),abs(l_poly[3] - self.ALCA_l_poly[3])) - if mx > 0.5: - mx = self.mx - self.mx = mx - else: - self.mx = 0 - - self.ALCA_l_poly = np.array(l_poly) - self.ALCA_r_poly = np.array(r_poly) + #where are we in alca as % ALCA_perc_complete = float(self.ALCA_step) / float(self.ALCA_total_steps) @@ -331,10 +330,7 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width): if self.ALCA_enabled and not (self.ALCA_direction == 0): if ALCA_DEBUG: print ALCA_perc_complete, self.ALCA_step,self.ALCA_total_steps - self.debug_alca(" ALCA enabled and direction not 0 -> let's go...") ALCA_increment = -3 if self.ALCA_cancelling else 1 - if ALCA_DEBUG: - print "increment", ALCA_increment self.ALCA_step += ALCA_increment if (self.ALCA_step < 0) or (self.ALCA_step >= self.ALCA_total_steps): #done so end ALCA @@ -343,86 +339,76 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width): else: #if between 20% and 80% of change is done, let's check if we are over the line if ALCA_line_check_low_limit < ALCA_perc_complete < ALCA_line_check_high_limit : - self.debug_alca("perc complete between ALCA_line_check_low_limit and ALCA_line_check_high_limit...") if self.ALCA_direction == -1: #if we are moving to the right - if (l_prob > ALCA_line_min_prob ) and (0 < l_poly[3] < (self.ALCA_lane_width / 3.)): - self.debug_alca("alca over the line...") + if (l_prob > ALCA_line_min_prob ) and (0. <= l_poly[3] <= (self.ALCA_lane_width /2.)): self.ALCA_over_line = True if self.ALCA_direction == 1: #if we are moving to the left - if (r_prob > ALCA_line_min_prob ) and ((-self.ALCA_lane_width / 3.) < r_poly[3] < 0 ): - self.debug_alca("alca over the line...") + if (r_prob > ALCA_line_min_prob ) and ((-self.ALCA_lane_width / 2.) <= r_poly[3] <= 0 ): self.ALCA_over_line = True elif ALCA_perc_complete >= ALCA_line_check_high_limit : - self.debug_alca("alca over the line (ALCA_line_check_high_limit )...") self.ALCA_over_line = True else: - self.debug_alca("alca not over the line (ALCA_line_check_low_limit)...") self.ALCA_over_line = False #make sure we always have the line we need in sight - if self.ALCA_over_line: - self.debug_alca("OVER THE LINE") prev_ALCA_use_visual = self.ALCA_use_visual if (not self.ALCA_over_line) and (((self.ALCA_direction == 1) and (l_prob < ALCA_line_min_prob)) or ((self.ALCA_direction == -1) and (r_prob < ALCA_line_min_prob))): - self.debug_alca("alca not over line, not using visual") self.ALCA_use_visual = False elif self.ALCA_over_line and (((self.ALCA_direction == 1) and (r_prob < ALCA_line_min_prob)) or ((self.ALCA_direction == -1) and (l_prob < ALCA_line_min_prob))): - self.debug_alca("alca over line, not using visual") self.ALCA_use_visual = False else: - self.debug_alca("alca using visual") self.ALCA_use_visual = True #did we just switch between visual and non-visual? if prev_ALCA_use_visual != self.ALCA_use_visual: self.reset_alca() - #maybe we need to change this with real time than assume 0.05s at 20Hz - #compute how much distance we did since last iteration - dx = 0.05 * (self.ALCA_vego_prev + v_ego) / 2 - #total distance traveled through the lane change - self.dx += dx - ix = 0. - #compute offset - if (not self.ALCA_error) and self.ALCA_use_visual: + from_center = 0. + left_to_move = 0. + if self.ALCA_enabled and not (self.ALCA_direction == 0): if self.ALCA_over_line: - if (self.ALCA_total_steps - self.ALCA_step <= 1) or (self.ALCA_over_line and ((self.ALCA_direction == 1) and (r_poly[3] < -ALCA_release_distance)) or ((self.ALCA_direction == -1) and (l_poly[3] > ALCA_release_distance))): - self.reset_alca() - self.ALCA_error = False - ix = self.ALCA_lane_width * float(self.ALCA_direction) / float(self.ALCA_total_steps) if self.ALCA_direction == 1: - #ix = -(self.ALCA_lane_width/2 + r_poly[3]) / float(self.ALCA_total_steps - self.ALCA_step) - self.ALCA_OFFSET_C3 = ix * float (self.ALCA_step) - self.ALCA_lane_width + from_center = self.ALCA_lane_width / 2 - r_poly[3] else: - #ix = -(self.ALCA_lane_width/2 - l_poly[3]) / float(self.ALCA_total_steps - self.ALCA_step) - self.ALCA_OFFSET_C3 = ix * float (self.ALCA_step) + self.ALCA_lane_width - #self.ALCA_OFFSET_C3 = ix * float(self.ALCA_total_steps - self.ALCA_step -1) - self.ALCA_OFFSET_C2 = self.mx * float(self.ALCA_direction) / dx + from_center = self.ALCA_lane_width / 2 + l_poly[3] else: - ix = self.ALCA_lane_width * float(self.ALCA_direction) / float(self.ALCA_total_steps) - self.ALCA_OFFSET_C3 = ix * float (self.ALCA_step) - self.ALCA_OFFSET_C2 = self.mx * float(self.ALCA_direction) / dx + if self.ALCA_direction == 1: + from_center = self.ALCA_lane_width / 2 - l_poly[3] + else: + from_center = self.ALCA_lane_width / 2 + r_poly[3] + if from_center < 0.: + from_center += self.ALCA_lane_width /2 + left_to_move = self.ALCA_lane_width - from_center + steps_left = self.ALCA_total_steps - self.ALCA_step + self.ALCA_OFFSET_C2 = float(self.ALCA_direction * left_to_move) / float(steps_left * 0.05 * (self.ALCA_vego_prev + v_ego) / 2.) + if ALCA_DEBUG: + debug_string = DEBUG_INFO.format(step=self.ALCA_step,total_steps=self.ALCA_total_steps,ALCA_direction=self.ALCA_direction,ALCA_use_visual=self.ALCA_use_visual,ALCA_over_line=self.ALCA_over_line,ALCA_lane_width=self.ALCA_lane_width, left_to_move=left_to_move, from_center=from_center, ALCA_OFFSET_C2=self.ALCA_OFFSET_C2) + self.debug_alca(debug_string) else: - self.ALCA_OFFSET_C3 = 0. self.ALCA_OFFSET_C2 = 0. - + if (not self.ALCA_error) and self.ALCA_use_visual: + if self.ALCA_over_line: + if (self.ALCA_total_steps - self.ALCA_step <= 1) or (self.ALCA_over_line and ((self.ALCA_direction == 1) and (r_poly[3] < -ALCA_release_distance)) or ((self.ALCA_direction == -1) and (l_poly[3] > ALCA_release_distance))): + self.reset_alca() + self.ALCA_error = False + if (self.ALCA_direction == 1 and not self.ALCA_over_line) or (self.ALCA_direction == -1 and self.ALCA_over_line): r_poly = np.array(l_poly) l_prob = 1 r_prob = l_prob - r_poly[3] = l_poly[3] - self.ALCA_lane_width elif (self.ALCA_direction == -1 and not self.ALCA_over_line) or (self.ALCA_direction == 1 and self.ALCA_over_line): l_poly = np.array(r_poly) r_prob = 1 l_prob = r_prob - l_poly[3] = r_poly[3] + self.ALCA_lane_width - l_poly[3] += self.ALCA_OFFSET_C3 - r_poly[3] += self.ALCA_OFFSET_C3 + l_poly[3] = self.ALCA_lane_width / 2 + r_poly[3] = -self.ALCA_lane_width / 2 + p_poly[3] = 0 l_poly[2] += self.ALCA_OFFSET_C2 r_poly[2] += self.ALCA_OFFSET_C2 + p_poly[2] += self.ALCA_OFFSET_C2 else: self.reset_alca() self.ALCA_error = False @@ -435,5 +421,6 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width): else: lane_width = self.ALCA_lane_width - return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width + self.send_state() + return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width, p_poly diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 8bb6c5bd5..3195e87a9 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -630,6 +630,9 @@ def update(self, enabled, CS, frame, actuators, \ can_sends.append(teslacan.create_fake_IC_msg()) idx = frame % 16 cruise_btn = None + # send enabled ethernet every 0.2 sec + if frame % 20 == 0: + can_sends.append(teslacan.create_enabled_eth_msg(1)) if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available: cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_for_cc, self.speedlimit_valid, \ diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 5a56d1f47..67d83ca56 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -210,7 +210,7 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret.openpilotLongitudinalControl = True ret.steerLimitAlert = False ret.startAccel = 0.5 - ret.steerRateCost = 0.6 + ret.steerRateCost = 0.7 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index 4966615ed..0934f5645 100644 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -7,6 +7,7 @@ from selfdrive.services import service_list import selfdrive.messaging as messaging from selfdrive.car.tesla.readconfig import CarSettings +from selfdrive.tinklad.tinkla_interface import TinklaClient #RADAR_A_MSGS = list(range(0x371, 0x37F , 3)) #RADAR_B_MSGS = list(range(0x372, 0x37F, 3)) @@ -15,6 +16,8 @@ RADAR_B_MSGS = list(range(0x311, 0x36F, 3)) OBJECT_MIN_PROBABILITY = 20. CLASS_MIN_PROBABILITY = 20. +RADAR_MESSAGE_FREQUENCY = 0.050 * 1e9 #time in ns, radar sends data at 0.06 s +VALID_MESSAGE_COUNT_THRESHOLD = 4 # Tesla Bosch firmware has 32 objects in all objects or a selected set of the 5 we should look at @@ -41,12 +44,15 @@ def _create_radard_can_parser(): [0] * msg_a_n + [0.] * msg_a_n + [0.] * msg_b_n + [0] * msg_b_n + [0] * msg_b_n + [0.] * msg_b_n + [0.] * msg_b_n +[0.] * msg_b_n + [0]* msg_b_n) - checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)) + checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [60]*(msg_a_n + msg_b_n)) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) class RadarInterface(object): + + tinklaClient = TinklaClient() + def __init__(self,CP): # radar self.pts = {} @@ -56,6 +62,7 @@ def __init__(self,CP): self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() + self.canErrorCounter = 0 if self.useTeslaRadar: self.pts = {} self.extPts = {} @@ -65,27 +72,32 @@ def __init__(self,CP): self.logcan = messaging.sub_sock(service_list['can'].port) self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 - self.trigger_msg = RADAR_B_MSGS[-1] + self.trigger_start_msg = RADAR_A_MSGS[0] + self.trigger_end_msg = RADAR_B_MSGS[-1] def update(self, can_strings): - # in Bosch radar and we are only steering for now, so sleep 0.05s to keep # radard at 20Hz and return no points if not self.useTeslaRadar: time.sleep(0.05) return car.RadarData.new_message(),self.extPts.values() + tm = int(sec_since_boot() * 1e9) if can_strings != None: vls = self.rcp.update_strings(tm, can_strings) self.updated_messages.update(vls) - if self.trigger_msg not in self.updated_messages: + + if self.trigger_start_msg not in self.updated_messages: + return None,None + + if self.trigger_end_msg not in self.updated_messages: return None,None rr,rrext = self._update(self.updated_messages) - self.updated_messages.clear() + #self.updated_messages.clear() return rr,rrext @@ -99,6 +111,10 @@ def _update(self, updated_messages): del self.extPts[message] continue cpt = self.rcp.vl[message] + cpt2 = self.rcp.vl[message+1] + # ensure the two messages are from the same frame reading + if cpt['Index'] != cpt2['Index2']: + continue if (cpt['LongDist'] >= BOSCH_MAX_DIST) or (cpt['LongDist']==0) or (not cpt['Tracked']): self.valid_cnt[message] = 0 # reset counter if message in self.pts: @@ -112,15 +128,11 @@ def _update(self, updated_messages): del self.pts[message] del self.extPts[message] - #score = self.rcp.vl[ii+16]['SCORE'] - #print ii, self.valid_cnt[ii], cpt['Valid'], cpt['LongDist'], cpt['LatDist'] - # radar point only valid if it's a valid measurement and score is above 50 # bosch radar data needs to match Index and Index2 for validity # also for now ignore construction elements if (cpt['Valid'] or cpt['Tracked'])and (cpt['LongDist']>0) and (cpt['LongDist'] < BOSCH_MAX_DIST) and \ - (cpt['Index'] == self.rcp.vl[message+1]['Index2']) and (self.valid_cnt[message] > 5) and \ - (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): # and (self.rcp.vl[ii+1]['Class'] < 4): # and ((self.rcp.vl[ii+1]['MovingState']<3) or (self.rcp.vl[ii+1]['Class'] > 0)): + (self.valid_cnt[message] > VALID_MESSAGE_COUNT_THRESHOLD) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): if message not in self.pts and ( cpt['Tracked']): self.pts[message] = car.RadarData.RadarPoint.new_message() self.pts[message].trackId = self.trackId @@ -134,14 +146,15 @@ def _update(self, updated_messages): self.pts[message].yRel = cpt['LatDist'] - self.radarOffset # in car frame's y axis, left is positive self.pts[message].vRel = cpt['LongSpeed'] self.pts[message].aRel = cpt['LongAccel'] - self.pts[message].yvRel = self.rcp.vl[message+1]['LatSpeed'] + self.pts[message].yvRel = cpt2['LatSpeed'] self.pts[message].measured = bool(cpt['Meas']) - self.extPts[message].dz = self.rcp.vl[message+1]['dZ'] - self.extPts[message].movingState = self.rcp.vl[message+1]['MovingState'] - self.extPts[message].length = self.rcp.vl[message+1]['Length'] + self.extPts[message].dz = cpt2['dZ'] + self.extPts[message].movingState = cpt2['MovingState'] + self.extPts[message].length = cpt2['Length'] self.extPts[message].obstacleProb = cpt['ProbObstacle'] + self.extPts[message].timeStamp = int(self.rcp.ts[message+1]['Index2']) if self.rcp.vl[message+1]['Class'] >= CLASS_MIN_PROBABILITY: - self.extPts[message].objectClass = self.rcp.vl[message+1]['Class'] + self.extPts[message].objectClass = cpt2['Class'] # for now we will use class 0- unknown stuff to show trucks # we will base that on being a class 1 and length of 2 (hoping they meant width not length, but as germans could not decide) # 0-unknown 1-four wheel vehicle 2-two wheel vehicle 3-pedestrian 4-construction element @@ -159,14 +172,22 @@ def _update(self, updated_messages): errors = [] if not self.rcp.can_valid: errors.append("canError") - ret.errors = errors + self.tinklaClient.logCANErrorEvent(source="radar_interface", canMessage=0, additionalInformation="Invalid CAN Count") + self.canErrorCounter += 1 + else: + self.canErrorCounter = 0 + #BB: Only trigger canError for 3 consecutive errors + if self.canErrorCounter > 2: + ret.errors = errors + else: + ret.errors = [] return ret,self.extPts.values() # radar_interface standalone tester if __name__ == "__main__": - CP = car.CarParams() + CP = None RI = RadarInterface(CP) while 1: ret,retext = RI.update(can_strings = None) print(chr(27) + "[2J") - print(ret) + print(ret,retext) diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 84631a0bb..a26e29693 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -56,6 +56,13 @@ def create_pedal_command_msg(accelCommand, enable, idx): (int_accelCommand2 >> 8) & 0xFF, int_accelCommand2 & 0XFF,((enable << 7) + idx) & 0xFF) struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) return [msg_id, 0, msg.raw, 2] + +def create_enabled_eth_msg(status): + msg_id = 0x018 + msg_len = 1 + msg = create_string_buffer(msg_len) + struct.pack_into('B', msg, 0, status) + return [msg_id, 0, msg.raw, 0] def create_fake_IC_msg(): msg_id = 0x649 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3a731f767..8838d2771 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,11 +27,12 @@ from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter +from selfdrive.tinklad.tinkla_interface import TinklaClient + ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState - def isActive(state): """Check if the actuators are enabled""" return state in [State.enabled, State.softDisabling] @@ -261,7 +262,7 @@ def state_control(frame, rcv_frame, plan, path_plan, live_params, CS, CP, state, actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan) + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan, live_params) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -414,6 +415,13 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca return CC, events_bytes +def logAllAliveAndValidInfoToTinklad(sm, tinklaClient): + areAllAlive, aliveProcessName, aliveCount = sm.all_alive() + areAllValid, validProcessName, validCount = sm.all_valid() + if not areAllAlive: + tinklaClient.logProcessCommErrorEvent(source="carcontroller", processName=aliveProcessName, count=aliveCount, eventType="Not Alive") + else: + tinklaClient.logProcessCommErrorEvent(source="carcontroller", processName=validProcessName, count=validCount, eventType="Not Valid") def controlsd_thread(gctx=None): gc.disable() @@ -423,6 +431,8 @@ def controlsd_thread(gctx=None): params = Params() + tinklaClient = TinklaClient() + # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) @@ -515,6 +525,7 @@ def controlsd_thread(gctx=None): # Create alerts if not sm.all_alive_and_valid(): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + logAllAliveAndValidInfoToTinklad(sm=sm, tinklaClient=tinklaClient) if not sm['pathPlan'].mpcSolutionValid: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: @@ -529,6 +540,7 @@ def controlsd_thread(gctx=None): events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + tinklaClient.logCANErrorEvent(source="carcontroller", canMessage=0, additionalInformation="Invalid CAN") if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 28dc1e903..e29076d48 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -24,7 +24,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): class LanePlanner(object): - def __init__(self): + def __init__(self,shouldUseAlca): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] self.p_poly = [0., 0., 0., 0.] @@ -40,7 +40,9 @@ def __init__(self): self._path_pinv = compute_path_pinv() self.x_points = np.arange(50) - self.ALCAMP = ALCAModelParser() + self.shouldUseAlca = shouldUseAlca + if shouldUseAlca: + self.ALCAMP = ALCAModelParser() def parse_model(self, md): if len(md.leftLane.poly): @@ -70,8 +72,8 @@ def update_lane(self, v_ego, md, alca ): (1 - self.lane_width_certainty) * speed_lane_width # ALCA integration - if alca: - self.r_poly,self.l_poly,self.r_prob,self.l_prob,self.lane_width = self.ALCAMP.update(v_ego, md, np.array(self.r_poly), np.array(self.l_poly), self.r_prob, self.l_prob, self.lane_width) + if self.shouldUseAlca and alca: + self.r_poly,self.l_poly,self.r_prob,self.l_prob,self.lane_width, self.p_poly = self.ALCAMP.update(v_ego, md, np.array(self.r_poly), np.array(self.l_poly), self.r_prob, self.l_prob, self.lane_width, self.p_poly) self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 595d98912..5d5f76234 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -14,7 +14,7 @@ def __init__(self, CP): def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan, live_params): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) @@ -24,7 +24,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste pid_log.active = False self.pid.reset() else: - self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + angle_bias = live_params.angleOffset - live_params.angleOffsetAverage + self.angle_steers_des = path_plan.angleSteers #+ angle_bias # get from MPC/PathPlanner steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index ab1bcae59..275ab6b12 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -11,7 +11,6 @@ from selfdrive.controls.lib.lane_planner import LanePlanner import selfdrive.messaging as messaging from selfdrive.car.tesla.readconfig import CarSettings -from cereal import tesla LOG_MPC = os.environ.get('LOG_MPC', False) @@ -24,13 +23,12 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ class PathPlanner(object): def __init__(self, CP): - self.LP = LanePlanner() + 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.alca = messaging.pub_sock(service_list['alcaState'].port) self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 @@ -135,18 +133,6 @@ def update(self, sm, CP, VM): self.plan.send(plan_send.to_bytes()) - alca_state = tesla.ALCAState.new_message() - #ALCA params - alca_state.alcaDirection = int(self.LP.ALCAMP.ALCA_direction) - alca_state.alcaError = bool(self.LP.ALCAMP.ALCA_error) - alca_state.alcaCancelling = bool(self.LP.ALCAMP.ALCA_cancelling) - alca_state.alcaEnabled = bool(self.LP.ALCAMP.ALCA_enabled) - alca_state.alcaLaneWidth = float(self.LP.ALCAMP.ALCA_lane_width) - alca_state.alcaStep = int(self.LP.ALCAMP.ALCA_step) - alca_state.alcaTotalSteps = int(self.LP.ALCAMP.ALCA_total_steps) - - self.alca.send(alca_state.to_bytes()) - if LOG_MPC: dat = messaging.new_message() dat.init('liveMpc') diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 03abcf461..7e80abc1a 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -2,6 +2,7 @@ import numpy as np import numpy.matlib import importlib +import zmq from collections import defaultdict, deque import selfdrive.messaging as messaging @@ -10,11 +11,10 @@ from selfdrive.config import RADAR_TO_CENTER from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.swaglog import cloudlog -from cereal import car,tesla +from cereal import car,log,tesla from common.params import Params from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL from selfdrive.car.tesla.readconfig import read_config_file,CarSettings -from selfdrive.controls.lib.lane_planner import LanePlanner DEBUG = False @@ -108,11 +108,14 @@ def __init__(self, mocked, RI): 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 update(self, frame, delay, sm, rr, has_radar,MP,rrext): + def update(self, frame, delay, sm, rr, has_radar,rrext): self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) use_tesla_radar = CarSettings().get_value("useTeslaRadar") if sm.updated['controlsState']: @@ -123,7 +126,13 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): if sm.updated['model']: self.ready = True - path_y = np.polyval(MP.d_poly, self.path_x) + for socket, _ in self.poller.poll(0): + if socket is self.pathPlanSocket: + pp = messaging.recv_one(self.pathPlanSocket).pathPlan + self.lane_width = pp.laneWidth + self.dPoly = pp.dPoly + + path_y = np.polyval(self.dPoly, self.path_x) ar_pts = {} for pt in rr.points: @@ -208,11 +217,10 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): datrl.v4Vrel = float(0.) datrl.v4Dy = float(0.) datrl.v4Id = int(0) - lane_offset = 0. #MP.lane_width - lane_width = 3. #BB: static for now, needs to be computed + lane_offset = 0. #LEFT LANE if self.RI.TRACK_LEFT_LANE and use_tesla_radar: - ll_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(-lane_width) for iden in idens]) + ll_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(-self.lane_width) for iden in idens]) # If we have multiple points, cluster them if len(ll_track_pts) > 1: ll_cluster_idxs = cluster_points_centroid(ll_track_pts, 2.5) @@ -235,7 +243,7 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): print(i) # *** extract the lead car *** ll_lead_clusters = [c for c in ll_clusters - if c.is_potential_lead_dy(self.v_ego,-lane_width)] + if c.is_potential_lead_dy(self.v_ego,-self.lane_width)] ll_lead_clusters.sort(key=lambda x: x.dRel) ll_lead_len = len(ll_lead_clusters) ll_lead1_truck = (len([c for c in ll_lead_clusters @@ -267,7 +275,7 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): datrl.v2Id = int(ll_lead2_clusters[0].track_id % 32) #RIGHT LANE if self.RI.TRACK_RIGHT_LANE and use_tesla_radar: - rl_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(lane_width) for iden in idens]) + rl_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(self.lane_width) for iden in idens]) # If we have multiple points, cluster them if len(rl_track_pts) > 1: rl_cluster_idxs = cluster_points_centroid(rl_track_pts, 2.5) @@ -290,7 +298,7 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): print(i) # *** extract the lead car *** rl_lead_clusters = [c for c in rl_clusters - if c.is_potential_lead_dy(self.v_ego,lane_width)] + if c.is_potential_lead_dy(self.v_ego,self.lane_width)] rl_lead_clusters.sort(key=lambda x: x.dRel) rl_lead_len = len(rl_lead_clusters) rl_lead1_truck = (len([c for c in rl_lead_clusters @@ -349,12 +357,6 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext): datext.lead2trackId = l2x['trackId'] datext.lead2oClass = l2x['oClass'] datext.lead2length = l2x['length'] - #datext.lead1trackId = l1x.trackId - #datext.lead1oClass = l1x.oClass - #datext.lead1length = l1x.length - #datext.lead2trackId = l2x.trackId - #datext.lead2oClass = l2x.oClass - #datext.lead2length = l2x.length return dat, datext @@ -385,7 +387,6 @@ def radard_thread(gctx=None): rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(mocked, RI) - MP = LanePlanner() has_radar = not CP.radarOffCan or mocked last_md_ts = 0. @@ -403,11 +404,9 @@ def radard_thread(gctx=None): if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo - if sm.updated['model']: - MP.update(v_ego, sm['model'], False) - dat,datext = RD.update(rk.frame, RI.delay, sm, rr, has_radar, MP, rrext) + dat,datext = RD.update(rk.frame, RI.delay, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining*1000. radarState.send(dat.to_bytes()) diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index c4929599a..97439537a 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -162,16 +162,67 @@ def update_msgs(self, cur_time, msgs): self.alive[s] = True def all_alive(self, service_list=None): + """Returns alive state for tracked processes. + + Args: + service_list (list): Optional service list. + + Returns: + tuple: areAllAlive, processName, count + """ if service_list is None: # check all service_list = self.alive.keys() - return all(self.alive[s] for s in service_list) + areAllAlive = True + processName = "" + count = 0 + for s in service_list: + if not self.alive[s]: + areAllAlive = False + processName = s + count = self.alive_cnt[s] + break + return (areAllAlive, processName, count) def all_valid(self, service_list=None): + """Returns valid state for tracked processes. + + Args: + service_list (list): Optional service list. + + Returns: + tuple: areAllValid, processName, count + """ if service_list is None: # check all service_list = self.valid.keys() - return all(self.valid[s] for s in service_list) + areAllValid = True + processName = "" + count = 0 + for s in service_list: + if not self.valid[s]: + areAllValid = False + processName = s + count = self.valid_cnt[s] + break + return (areAllValid, processName, count) + + def all_alive_and_valid_with_info(self, service_list=None): + """Returns alive and valid state for tracked processes. + + Args: + service_list (list): Optional service list. + + Returns: + tuple: areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, validCount + """ + if service_list is None: # check all + service_list = self.alive.keys() + areAllAlive, aliveProcessName, aliveCount = self.all_alive(service_list=service_list) + areAllValid, validProcessName, validCount = self.all_valid(service_list=service_list) + return (areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, ) def all_alive_and_valid(self, service_list=None): if service_list is None: # check all service_list = self.alive.keys() - return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list) + areAllAlive, aliveProcessName, aliveCount = self.all_alive(service_list=service_list) + areAllValid, validProcessName, validCount = self.all_valid(service_list=service_list) + return areAllAlive and areAllValid diff --git a/selfdrive/tinklad/tinkla_interface.py b/selfdrive/tinklad/tinkla_interface.py index 59efe5861..a9b4bfc3c 100644 --- a/selfdrive/tinklad/tinkla_interface.py +++ b/selfdrive/tinklad/tinkla_interface.py @@ -5,6 +5,7 @@ import zmq import datetime import tinklad +import time ## For helpers: import traceback @@ -20,10 +21,20 @@ def now_iso8601(): class TinklaClient(): sock = None pid = None + lastCanErrorTimestamp = 0 + lastProcessErrorTimestamp = 0 eventCategoryKeys = tinklad.TinklaInterfaceEventCategoryKeys() messageTypeKeys = tinklad.TinklaInterfaceMessageKeys() actions = tinklad.TinklaInterfaceActions() + + # Configurable: + # Note: If throttling, events are dropped + shouldThrottleCanErrorEvents = True + shouldThrottleProcessCommErrorEvents = True + # Setting to every 30min for now, because we're getting a bunch of plan, pathPlan issues. + # Should change to around every 1min in the future when this is resolved + throttlingPeriodInSeconds = (60*30) # One event every `throttlingPeriodInSeconds` def start_client(self): if os.getpid() == self.pid: @@ -90,43 +101,70 @@ def attemptToSendPendingMessages(self): ## Helpers: - def logCrashStackTraceEvent(self, dongleId = None): - if dongleId is None: - dongleId = self.dongleId + def logCrashStackTraceEvent(self, openPilotId = None): + if openPilotId is None: + openPilotId = self.openPilotId event = tinkla.Interface.Event.new_message( - openPilotId=dongleId, + openPilotId=openPilotId, source="n/a", category=self.eventCategoryKeys.crash, name="crash", ) trace = traceback.format_exc().replace('"', '`').replace("'", '`') - userInfo = "User Handle: %s OpenPilotId: %s" % (self.userHandle, self.dongleId) + userInfo = "User Handle: %s \nOpenPilotId: %s" % (self.userHandle, self.openPilotId) gitInfo = "Git Remote: %s\nBranch: %s\nCommit: %s" % (self.gitRemote, self.gitBranch, self.gitHash) event.value.textValue="%s\n%s\n%s" % (userInfo, gitInfo, trace) self.logUserEvent(event) - def logCANErrorEvent(self, canMessage, additionalInformation, dongleId = None): - if dongleId is None: - dongleId = self.dongleId + def logCANErrorEvent(self, source, canMessage, additionalInformation, openPilotId = None): + if self.shouldThrottleCanErrorEvents: + now = time.time() + if now - self.lastCanErrorTimestamp < self.throttlingPeriodInSeconds: + return + self.lastCanErrorTimestamp = now + + if openPilotId is None: + openPilotId = self.openPilotId event = tinkla.Interface.Event.new_message( - openPilotId=dongleId, - source=hex(canMessage), + openPilotId=openPilotId, + source=source, category=self.eventCategoryKeys.canError, name="CAN Error", ) canInfo = "Can Message: {0}".format(hex(canMessage)) - userInfo = "User Handle: %s OpenPilotId: %s" % (self.userHandle, self.dongleId) + userInfo = "User Handle: %s \nOpenPilotId: %s" % (self.userHandle, self.openPilotId) gitInfo = "Git Remote: %s\nBranch: %s\nCommit: %s" % (self.gitRemote, self.gitBranch, self.gitHash) event.value.textValue="%s\n%s\n%s\n%s" % (userInfo, gitInfo, canInfo, additionalInformation) self.logUserEvent(event) + def logProcessCommErrorEvent(self, source, processName, count, eventType, openPilotId = None): + if self.shouldThrottleProcessCommErrorEvents: + now = time.time() + if now - self.lastProcessErrorTimestamp < self.throttlingPeriodInSeconds: + return + self.lastProcessErrorTimestamp = now + + if openPilotId is None: + openPilotId = self.openPilotId + event = tinkla.Interface.Event.new_message( + openPilotId=openPilotId, + source=processName, + category=self.eventCategoryKeys.processCommError, + name="Process Comm Error", + ) + additionalInformation = "Process: '%s' \nType: '%s' \nCount: '%d' \nSource: '%s'" % (processName, eventType, count, source) + userInfo = "User Handle: %s \nOpenPilotId: %s" % (self.userHandle, self.openPilotId) + gitInfo = "Git Remote: %s\nBranch: %s\nCommit: %s" % (self.gitRemote, self.gitBranch, self.gitHash) + event.value.textValue="%s\n%s\n%s" % (userInfo, gitInfo, additionalInformation) + self.logUserEvent(event) + def print_msg(self, message): print(message) def __init__(self): carSettings = CarSettings() params = Params() - self.dongleId = params.get("DongleId") + self.openPilotId = params.get("DongleId") self.userHandle = carSettings.userHandle self.gitRemote = params.get("GitRemote") self.gitBranch = params.get("GitBranch") diff --git a/selfdrive/tinklad/tinkladTestClient.py b/selfdrive/tinklad/tinkladTestClient.py index baebd5151..1b0be87d8 100644 --- a/selfdrive/tinklad/tinkladTestClient.py +++ b/selfdrive/tinklad/tinkladTestClient.py @@ -10,10 +10,13 @@ class TinklaTestClient(): def __init__(self): #self.start_server() self.tinklaClient = TinklaClient() + openPilotId = "test_openpilotId" + source = "tinkladTestClient" + userHandle = "test_user_handle" info = tinkla.Interface.UserInfo.new_message( - openPilotId="test_openpilotId", - userHandle="test_user_handle", + openPilotId=openPilotId, + userHandle=userHandle, gitRemote="test_github.com/something", gitBranch="test_gitbranch", gitHash="test_123456" @@ -24,8 +27,8 @@ def __init__(self): print("Info Time Elapsed = %d" % (elapsed_time_us)) event = tinkla.Interface.Event.new_message( - openPilotId="test_openpilotId", - source="unittest", + openPilotId=openPilotId, + source=source, category=self.tinklaClient.eventCategoryKeys.userAction, name="pull_stalk", ) @@ -36,18 +39,24 @@ def __init__(self): print("Event Time Elapsed = %d" % (elapsed_time_us)) carsettings = CarSettings("./bb_openpilot_config.cfg") - userHandle = carsettings.userHandle + carsettings.userHandle = userHandle print("userHandle = '%s'" % (userHandle)) print("attemptToSendPendingMessages") self.tinklaClient.attemptToSendPendingMessages() print("send crash log") - self.tinklaClient.logCrashStackTraceEvent(dongleId="test_openpilotId") + self.tinklaClient.logCrashStackTraceEvent(openPilotId=openPilotId) print("send can error") - self.tinklaClient.logCANErrorEvent(canMessage=123, additionalInformation="test can error logging", dongleId="test_openpilotId") + self.tinklaClient.logCANErrorEvent(source=source, canMessage=1, additionalInformation="test can error logging", openPilotId=openPilotId) + time.sleep(1) + self.tinklaClient.logCANErrorEvent(source=source, canMessage=2, additionalInformation="test can error logging", openPilotId=openPilotId) + + print("send process comm error") + self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere1", count=10, eventType="Not Alive", openPilotId=openPilotId) + time.sleep(1) + self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere2", count=10, eventType="Not Alive", openPilotId=openPilotId) if __name__ == "__main__": TinklaTestClient() - \ No newline at end of file