diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a7e3c0211d..bf19801a87 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -208,23 +208,17 @@ class RadarD: self.ready = False - def update(self, sm: messaging.SubMaster, rr): + def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) - radar_points = [] - radar_errors = [] - if rr is not None: - radar_points = rr.points - radar_errors = rr.errors - if sm.recv_frame['carState'] != self.last_v_ego_frame: self.v_ego = sm['carState'].vEgo self.v_ego_hist.append(self.v_ego) self.last_v_ego_frame = sm.recv_frame['carState'] ar_pts = {} - for pt in radar_points: + for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** @@ -245,10 +239,10 @@ class RadarD: self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) # *** publish radarState *** - self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0 + self.radar_state_valid = sm.all_checks() and len(rr.errors) == 0 self.radar_state = log.RadarState.new_message() self.radar_state.mdMonoTime = sm.logMonoTime['modelV2'] - self.radar_state.radarErrors = list(radar_errors) + self.radar_state.radarErrors = list(rr.errors) self.radar_state.carStateMonoTime = sm.logMonoTime['carState'] if len(sm['modelV2'].temporalPose.trans): @@ -283,7 +277,7 @@ class RadarD: # fuses camera and radar data for best lead detection -def main(): +def main() -> None: config_realtime_process(5, Priority.CTRL_LOW) # wait for stats about the car to come in from controls @@ -307,7 +301,7 @@ def main(): while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) - rr = RI.update(can_capnp_to_list(can_strings)) + rr: car.RadarData | None = RI.update(can_capnp_to_list(can_strings)) sm.update(0) if rr is None: continue