Exemple #1
0
  def update_app(self):
    put_nonblocking('dp_is_updating', '1')
    if self.has_own_apk:
      try:
        subprocess.check_output(["pm","install","-r",self.own_apk])
        self.is_installed = True
      except subprocess.CalledProcessError as e:
        self.is_installed = False
    else:
      apk = self.app + ".apk"
      apk_path = "/sdcard/" + apk
      try:
        os.remove(apk_path)
      except (OSError, FileNotFoundError) as e:
        pass

      self.uninstall_app()
      # if local_version is not None:
      #   try:
      #     subprocess.check_output(["pm","uninstall", self.app], stderr=subprocess.STDOUT, shell=True)
      #   except subprocess.CalledProcessError as e:
      #     pass
      try:
        url = "https://raw.githubusercontent.com/dragonpilot-community/apps/%s/%s" % (apk, apk)
        subprocess.check_output(["curl","-o", apk_path,"-LJO", url])
        subprocess.check_output(["pm","install","-r",apk_path])
        self.is_installed = True
      except subprocess.CalledProcessError as e:
        self.is_installed = False
      try:
        os.remove(apk_path)
      except (OSError, FileNotFoundError) as e:
        pass
    put_nonblocking('dp_is_updating', '0')
Exemple #2
0
  def handle_cam_odom(self, trans, rot, trans_std, rot_std):
    straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
    certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
                        (self.valid_blocks < INPUTS_NEEDED))
    if straight_and_fast and certain_if_calib:
      # intrinsics are not eon intrinsics, since this is calibrated frame
      intrinsics = intrinsics_from_vp(self.vp)
      new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
      new_vp = new_vp[:2]/new_vp[2]
      new_vp = sanity_clip(new_vp)

      self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
      self.idx = (self.idx + 1) % BLOCK_SIZE
      if self.idx == 0:
        self.block_idx += 1
        self.valid_blocks = max(self.block_idx, self.valid_blocks)
        self.block_idx = self.block_idx % INPUTS_WANTED
      if self.valid_blocks > 0:
        self.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
      self.update_status()

      if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
        calib = get_calib_from_vp(self.vp)
        cal_params = {"calib_radians": list(calib),
                      "valid_blocks": self.valid_blocks}
        put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
      return new_vp
    else:
      return None
  def update_status(self):
    if self.valid_blocks > 0:
      max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0))
      min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0))
      self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
    else:
      self.calib_spread = np.zeros(3)

    if self.valid_blocks < INPUTS_NEEDED:
      self.cal_status = Calibration.UNCALIBRATED
    elif is_calibration_valid(self.rpy):
      self.cal_status = Calibration.CALIBRATED
    else:
      self.cal_status = Calibration.INVALID

    # If spread is too high, assume mounting was changed and reset to last block.
    # Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model.
    if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
      self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)

    write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
    if self.param_put and write_this_cycle:
      # TODO: this should use the liveCalibration struct from cereal
      cal_params = {"calib_radians": list(self.rpy),
                    "valid_blocks": int(self.valid_blocks)}
      put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
Exemple #4
0
  def update_status(self) -> None:
    valid_idxs = self.get_valid_idxs()
    if valid_idxs:
      rpys = self.rpys[valid_idxs]
      self.rpy = np.mean(rpys, axis=0)
      max_rpy_calib = np.array(np.max(rpys, axis=0))
      min_rpy_calib = np.array(np.min(rpys, axis=0))
      self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
    else:
      self.calib_spread = np.zeros(3)

    if self.valid_blocks < INPUTS_NEEDED:
      self.cal_status = Calibration.UNCALIBRATED
    elif is_calibration_valid(self.rpy):
      self.cal_status = Calibration.CALIBRATED
    else:
      self.cal_status = Calibration.INVALID

    # If spread is too high, assume mounting was changed and reset to last block.
    # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
    if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
      self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)

    write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
    if self.param_put and write_this_cycle:
      put_nonblocking("CalibrationParams", self.get_msg().to_bytes())
Exemple #5
0
  def handle_cam_odom(self, trans, rot, trans_std, rot_std):
    straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
    certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
                        (self.valid_blocks < INPUTS_NEEDED))
    if straight_and_fast and certain_if_calib:
      observed_rpy = np.array([0,
                               -np.arctan2(trans[2], trans[0]),
                               np.arctan2(trans[1], trans[0])])
      new_rpy = euler_from_rot(rot_from_euler(self.rpy).dot(rot_from_euler(observed_rpy)))
      new_rpy = sanity_clip(new_rpy)

      self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
      self.idx = (self.idx + 1) % BLOCK_SIZE
      if self.idx == 0:
        self.block_idx += 1
        self.valid_blocks = max(self.block_idx, self.valid_blocks)
        self.block_idx = self.block_idx % INPUTS_WANTED
      if self.valid_blocks > 0:
        self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
      self.update_status()

      # TODO: this should use the liveCalibration struct from cereal
      if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
        cal_params = {"calib_radians": list(self.rpy),
                      "valid_blocks": self.valid_blocks}
        put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
      return new_rpy
    else:
      return None
Exemple #6
0
  def Task(self, gctx=None):
    self.opkr_enable_softkey = int(params.get('OpkrRunSoftkey', encoding='utf8'))
    for x in program:
      nPos = x[0]        
      opkr_enable = int(params.get( x[1], encoding='utf8'))
      if self.navigation_is_running:
        if nPos == 2:
          opkr_enable = True

      if opkr_enable:
        print( '1:{} 2:{}'.format( x[1], x[2], program_runing[nPos] ) )
        self.all_kill()
        program_runing[nPos] = self.exec_app( opkr_enable, x[2], x[3])
        time.sleep(1)
        put_nonblocking( x[1], '0')
        if nPos == 2:
          self.opkr_enable_softkey = True
      else:
        time.sleep(0.1)


    self.navigation_is_running = 0
    # allow user to manually start/stop app
    if self.opkr_enable_softkey:
      put_nonblocking('OpkrRunSoftkey', '0')
      if not self.softkey_is_running:
        self.softkey_is_running = self.exec_app(self.opkr_enable_softkey, "com.gmd.hidesoftkeys", "com.gmd.hidesoftkeys.MainActivity")
      
    #threading.Timer( 0.3, self.Task ).start()
    time.sleep(0.3)
Exemple #7
0
    def handle_cam_odom(self, trans, rot, trans_std, rot_std):
        if ((trans[0] > MIN_SPEED_FILTER) and (trans_std[0] < MAX_SPEED_STD)
                and (abs(rot[2]) < MAX_YAW_RATE_FILTER)):
            # intrinsics are not eon intrinsics, since this is calibrated frame
            intrinsics = intrinsics_from_vp(self.vp)
            new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
            new_vp = new_vp[:2] / new_vp[2]
            new_vp = sanity_clip(new_vp)

            self.vps[self.block_idx] = (
                self.idx * self.vps[self.block_idx] +
                (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
            self.idx = (self.idx + 1) % BLOCK_SIZE
            if self.idx == 0:
                self.block_idx += 1
                self.valid_blocks = max(self.block_idx, self.valid_blocks)
                self.block_idx = self.block_idx % INPUTS_WANTED
            if self.valid_blocks > 0:
                self.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
            self.update_status()

            if self.param_put and ((self.idx == 0 and self.block_idx == 0)
                                   or self.just_calibrated):
                cal_params = {
                    "vanishing_point": list(self.vp),
                    "valid_blocks": self.valid_blocks
                }
                put_nonblocking("CalibrationParams",
                                json.dumps(cal_params).encode('utf8'))
            return new_vp
        else:
            return None
Exemple #8
0
 def cache_ephemeris(self, t: GPSTime):
   if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
     put_nonblocking(EPHEMERIS_CACHE, json.dumps(
       {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav},
       cls=CacheSerializer))
     cloudlog.debug("Cache saved")
     self.last_cached_t = t
Exemple #9
0
  def init_vars(self, dragonconf):
    self.is_enabled = getattr(dragonconf, self.enable_struct)

    if self.is_enabled:
      local_version = self.get_local_version()
      if local_version is not None:
        self.is_installed = True

      if self.has_own_apk and not self.is_installed:
        self.update_app()

      elif is_online:
        if local_version is None:
          self.update_app()
        else:
          remote_version = self.get_remote_version() if (auto_update and not self.own_apk) else local_version
          if remote_version is not None and local_version != remote_version:
            self.update_app()
      if self.is_installed:
        self.set_package_permissions()
    else:
      self.uninstall_app()

    if self.manual_ctrl_param is not None and getattr(dragonconf, self.manual_ctrl_struct) != self.MANUAL_IDLE:
      put_nonblocking(self.manual_ctrl_param, str(self.MANUAL_IDLE))
    self.init = True
Exemple #10
0
 def all_kill(self, enable = 0 ):
   put_nonblocking('OpkrRunSoftkey', '0')
   for x in program:
     nPos = x[0]
     put_nonblocking( x[1], '0')
     if program_runing[nPos]:
       program_runing[nPos] = 0
       self.exec_app( enable, x[2], x[3])
Exemple #11
0
  def update(self, c, can_strings):
    canMonoTimes = []
    buttonEvents = []

    # Process the most recent CAN message traffic, and check for validity
    # The camera CAN has no signals we use at this time, but we process it
    # anyway so we can test connectivity with can_valid
    self.cp.update_strings(can_strings)
    self.cp_cam.update_strings(can_strings)

    ret = self.CS.update(self.cp)
    ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
    ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False

    # Update the EON metric configuration to match the car at first startup,
    # or if there's been a change.
    if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
      put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")

    # Check for and process state-change events (button press or release) from
    # the turn stalk switch or ACC steering wheel/control stalk buttons.
    for button in self.CS.buttonStates:
      if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
        be = car.CarState.ButtonEvent.new_message()
        be.type = button
        be.pressed = self.CS.buttonStates[button]
        buttonEvents.append(be)

    events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport])

    # Vehicle health and operation safety checks
    if self.CS.parkingBrakeSet:
      events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
    if self.CS.steeringFault:
      events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))

    # Engagement and longitudinal control using stock ACC. Make sure OP is
    # disengaged if stock ACC is disengaged.
    if not ret.cruiseState.enabled:
      events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
    # Attempt OP engagement only on rising edge of stock ACC engagement.
    elif not self.cruise_enabled_prev:
      events.append(create_event('pcmEnable', [ET.ENABLE]))

    ret.events = events
    ret.buttonEvents = buttonEvents
    ret.canMonoTimes = canMonoTimes

    # update previous car states
    self.gas_pressed_prev = ret.gasPressed
    self.brake_pressed_prev = ret.brakePressed
    self.cruise_enabled_prev = ret.cruiseState.enabled
    self.displayMetricUnitsPrev = self.CS.displayMetricUnits
    self.buttonStatesPrev = self.CS.buttonStates.copy()

    self.CS.out = ret.as_reader()
    return self.CS.out
    def calculate(self, peripheralState, ignition):
        try:
            now = sec_since_boot()

            # If peripheralState is None, we're probably not in a car, so we don't care
            if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown:
                with self.integration_lock:
                    self.last_measurement_time = None
                    self.next_pulsed_measurement_time = None
                    self.power_used_uWh = 0
                return

            # Low-pass battery voltage
            self.car_voltage_instant_mV = peripheralState.voltage
            self.car_voltage_mV = (
                (peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) +
                (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
            statlog.gauge("car_voltage", self.car_voltage_mV / 1e3)

            # Cap the car battery power and save it in a param every 10-ish seconds
            self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh,
                                                0)
            self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh,
                                                CAR_BATTERY_CAPACITY_uWh)
            if now - self.last_save_time >= 10:
                put_nonblocking("CarBatteryCapacity",
                                str(int(self.car_battery_capacity_uWh)))
                self.last_save_time = now

            # First measurement, set integration time
            with self.integration_lock:
                if self.last_measurement_time is None:
                    self.last_measurement_time = now
                    return

            if ignition:
                # If there is ignition, we integrate the charging rate of the car
                with self.integration_lock:
                    self.power_used_uWh = 0
                    integration_time_h = (now -
                                          self.last_measurement_time) / 3600
                    if integration_time_h < 0:
                        raise ValueError(
                            f"Negative integration time: {integration_time_h}h"
                        )
                    self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W *
                                                      1e6 * integration_time_h)
                    self.last_measurement_time = now
            else:
                # Get current power draw somehow
                current_power = HARDWARE.get_current_power_draw()

                # Do the integration
                self._perform_integration(now, current_power)
        except Exception:
            cloudlog.exception("Power monitoring calculation failed")
Exemple #13
0
    def update(self, c, can_strings):
        canMonoTimes = []

        ret_arne182 = arne182.CarStateArne182.new_message()
        buttonEvents = []

        # Process the most recent CAN message traffic, and check for validity
        # The camera CAN has no signals we use at this time, but we process it
        # anyway so we can test connectivity with can_valid
        self.cp.update_strings(can_strings)
        self.cp_cam.update_strings(can_strings)

        ret = self.CS.update(self.cp)
        ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
        ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False

        # Update the EON metric configuration to match the car at first startup,
        # or if there's been a change.
        if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
            put_nonblocking("IsMetric",
                            "1" if self.CS.displayMetricUnits else "0")

        # Check for and process state-change events (button press or release) from
        # the turn stalk switch or ACC steering wheel/control stalk buttons.
        for button in self.CS.buttonStates:
            if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
                be = car.CarState.ButtonEvent.new_message()
                be.type = button
                be.pressed = self.CS.buttonStates[button]
                buttonEvents.append(be)

        events, events_arne182 = self.create_common_events(
            ret, extra_gears=[GEAR.eco, GEAR.sport])

        # Vehicle health and operation safety checks
        if self.CS.parkingBrakeSet:
            events.add(EventName.parkBrake)
        if self.CS.steeringFault:
            events.add(EventName.steerTempUnavailable)

        ret_arne182.events = events_arne182.to_msg()
        ret.events = events.to_msg()

        ret.buttonEvents = buttonEvents
        ret.canMonoTimes = canMonoTimes

        # update previous car states
        self.displayMetricUnitsPrev = self.CS.displayMetricUnits
        self.buttonStatesPrev = self.CS.buttonStates.copy()

        self.CS.out = ret.as_reader()
        return self.CS.out, ret_arne182.as_reader()
def update_custom_logic(msg):
    if msg.dragonConf.dpAssistedLcMinMph > msg.dragonConf.dpAutoLcMinMph:
        put_nonblocking('dp_auto_lc_min_mph',
                        str(msg.dragonConf.dpAssistedLcMinMph))
        msg.dragonConf.dpAutoLcMinMph = msg.dragonConf.dpAssistedLcMinMph
    if msg.dragonConf.dpAtl:
        msg.dragonConf.dpAllowGas = True
        msg.dragonConf.dpDynamicFollow = 0
        msg.dragonConf.dpAccelProfile = 0
        msg.dragonConf.dpGearCheck = False
    if msg.dragonConf.dpAppWaze or msg.dragonConf.dpAppHr:
        msg.dragonConf.dpDrivingUi = False
    if not msg.dragonConf.dpDriverMonitor:
        msg.dragonConf.dpUiFace = False
    return msg
Exemple #15
0
  def kill(self, force = False):
    if self.is_installed and (force or self.is_enabled):
      # app is manually ctrl, we record that
      if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_OFF:
        put_nonblocking(self.manual_ctrl_param, '0')
        self.manually_ctrled = True
        self.is_running = True

      # only kill app if it's running
      if force or self.is_running:
        if self.app_type == self.TYPE_SERVICE:
          self.appops_set(self.app, "android:mock_location", "deny")

        self.system("pkill %s" % self.app)
        self.is_running = False
Exemple #16
0
    def __init__(self, app, activity, enable_param, auto_run_param,
                 manual_ctrl_param, app_type, permissions, opts):
        self.app = app
        # main activity
        self.activity = activity
        # read enable param
        self.enable_param = enable_param
        # read auto run param
        self.auto_run_param = auto_run_param
        # read manual run param
        self.manual_ctrl_param = manual_ctrl_param
        # if it's a service app, we do not kill if device is too hot
        self.app_type = app_type
        # app permissions
        self.permissions = permissions
        # app options
        self.opts = opts

        self.is_installed = False
        self.is_enabled = True if params.get(self.enable_param,
                                             encoding='utf8') == "1" else False
        self.last_is_enabled = False
        self.is_auto_runnable = False
        self.is_running = False
        self.manual_ctrl_status = self.MANUAL_IDLE
        self.manually_ctrled = False

        if self.is_enabled:
            local_version = self.get_local_version()
            if local_version is not None:
                self.is_installed = True

            if not is_offline:
                remote_version = local_version
                if local_version is not None and auto_update:
                    remote_version = self.get_remote_version()
                if local_version is None or (remote_version is not None and
                                             local_version != remote_version):
                    self.update_app()
            if self.is_installed:
                self.set_package_permissions()
        else:
            self.uninstall_app()

        if self.manual_ctrl_param is not None:
            put_nonblocking(self.manual_ctrl_param, '0')
        self.last_ts = sec_since_boot()
Exemple #17
0
 def handle_cam_odom(self, log):
   trans, rot = log.trans, log.rot
   if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
     new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
     new_vp = new_vp[:2]/new_vp[2]
     self.vps.append(new_vp)
     self.vps = self.vps[-INPUTS_WANTED:]
     self.vp = np.mean(self.vps, axis=0)
     self.update_status()
     self.write_counter += 1
     if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
       cal_params = {"vanishing_point": list(self.vp),
                     "valid_points": len(self.vps)}
       put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
     return new_vp
   else:
     return None
Exemple #18
0
  def run(self, force = False):
    if self.is_installed and (force or self.is_enabled):
      # app is manually ctrl, we record that
      if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_ON:
        put_nonblocking(self.manual_ctrl_param, '0')
        put_nonblocking('dp_last_modified', str(floor(time.time())))
        self.manually_ctrled = True
        self.is_running = False

      # only run app if it's not running
      if force or not self.is_running:
        self.system("pm enable %s" % self.app)

        if self.app_type == self.TYPE_SERVICE:
          self.appops_set(self.app, "android:mock_location", "allow")
        self.system(self.start_cmd)
    self.is_running = True
Exemple #19
0
def update_custom_logic(msg):
    if msg.dragonConf.dpAtl:
        msg.dragonConf.dpAllowGas = True
        msg.dragonConf.dpFollowingProfileCtrl = False
        msg.dragonConf.dpAccelProfileCtrl = False
        msg.dragonConf.dpGearCheck = False
    if msg.dragonConf.dpLcMinMph > msg.dragonConf.dpLcAutoMinMph:
        put_nonblocking('dp_lc_auto_min_mph', str(msg.dragonConf.dpLcMinMph))
        msg.dragonConf.dpLcAutoMinMph = msg.dragonConf.dpLcMinMph
    # if msg.dragonConf.dpSrCustom <= 4.99 and msg.dragonConf.dpSrStock > 0:
    #   put_nonblocking('dp_sr_custom', str(msg.dragonConf.dpSrStock))
    #   msg.dragonConf.dpSrCustom = msg.dragonConf.dpSrStock
    # if msg.dragonConf.dpAppWaze or msg.dragonConf.dpAppHr:
    #   msg.dragonConf.dpDrivingUi = False
    # if not msg.dragonConf.dpDriverMonitor:
    #   msg.dragonConf.dpUiFace = False
    return msg
Exemple #20
0
  def data_sample(self):
    """Receive data from sockets and update carState"""

    # Update carState from CAN
    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
    CS = self.CI.update(self.CC, can_strs)

    self.sm.update(0)

    all_valid = CS.canValid and self.sm.all_alive_and_valid()
    if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION):
      if not self.read_only:
        self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
      self.initialized = True
      Params().put_bool("ControlsReady", True)

    # Check for CAN timeout
    if not can_strs:
      self.can_error_counter += 1
      self.can_rcv_error = True
    else:
      self.can_rcv_error = False

    # When the panda and controlsd do not agree on controls_allowed
    # we want to disengage openpilot. However the status from the panda goes through
    # another socket other than the CAN messages and one can arrive earlier than the other.
    # Therefore we allow a mismatch for two samples, then we trigger the disengagement.
    if not self.enabled:
      self.mismatch_counter = 0

    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
    if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
           if ps.safetyModel not in IGNORED_SAFETY_MODES):
      self.mismatch_counter += 1

    self.distance_traveled += CS.vEgo * DT_CTRL

    if self.jvePilotState.notifyUi:
      self.ui_notify()
    elif self.sm.updated['jvePilotUIState']:
      self.jvePilotState.carControl.autoFollow = self.sm['jvePilotUIState'].autoFollow
      self.jvePilotState.carControl.accEco = self.sm['jvePilotUIState'].accEco
      put_nonblocking("jvePilot.carState.accEco", str(self.sm['jvePilotUIState'].accEco))

    return CS
Exemple #21
0
def dragonpilot_set_params(params):
  # # remove deprecated params
  # for old, new in deprecated_conf.items():
  #   if params.get(old) is not None:
  #     if new is not None:
  #       old_val = str(params.get(old))
  #       new_val = old_val
  #       # invert the value if true
  #       if old in deprecated_conf_invert and deprecated_conf_invert[old] is True:
  #         new_val = "1" if old_val == "0" else "0"
  #       put_nonblocking(new, new_val)
  #     params.delete(old)

  # set params
  for key, val in default_conf.items():
    if params.get(key) is None and key not in deprecated_conf:
      put_nonblocking(key, str(val))
  put_nonblocking('DragonSupportedCars', get_support_car_list())
Exemple #22
0
    def run(self, force=False):
        if self.is_installed and (force or self.is_enabled):
            # app is manually ctrl, we record that
            if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_ON:
                put_nonblocking(self.manual_ctrl_param, '0')
                put_nonblocking('DragonLastModified', str(floor(time.time())))
                self.manually_ctrled = True
                self.is_running = False

            # only run app if it's not running
            if force or not self.is_running:
                self.system("pm enable %s" % self.app)

                if self.app_type == self.TYPE_GPS_SERVICE:
                    self.appops_set(self.app, "android:mock_location", "allow")

                if self.app_type in [self.TYPE_SERVICE, self.TYPE_GPS_SERVICE]:
                    self.system("am startservice %s/%s" %
                                (self.app, self.activity))
                else:
                    self.system("am start -n %s/%s" %
                                (self.app, self.activity))
        self.is_running = True
Exemple #23
0
def controlsd_thread(sm=None, pm=None, can_sock=None):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    params = Params()

    is_metric = params.get("IsMetric", encoding='utf8') == "1"
    is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
    passive = params.get("Passive", encoding='utf8') == "1"
    openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
                                          encoding='utf8') == "1"
    community_feature_toggle = params.get("CommunityFeaturesToggle",
                                          encoding='utf8') == "1"

    passive = passive or not openpilot_enabled_toggle

    # Passive if internet needed
    internet_needed = params.get("Offroad_ConnectivityNeeded",
                                 encoding='utf8') is not None
    passive = passive or internet_needed

    # Pub/Sub Sockets
    if pm is None:
        pm = messaging.PubMaster([
            'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
            'carParams'
        ])

    if sm is None:
        sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
                                  'model'])

    if can_sock is None:
        can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
        can_sock = messaging.sub_sock('can', timeout=can_timeout)

    # wait for health and CAN packets
    hw_type = messaging.recv_one(sm.sock['health']).health.hwType
    has_relay = hw_type in [HwType.blackPanda, HwType.uno]
    print("Waiting for CAN messages...")
    messaging.get_one_can(can_sock)

    CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)

    car_recognized = CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not chffrplus
    controller_available = CP.enableCamera and CI.CC is not None and not passive
    community_feature_disallowed = CP.communityFeature and not community_feature_toggle
    read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
    if read_only:
        CP.safetyModel = car.CarParams.SafetyModel.noOutput

    # Write CarParams for radard and boardd safety mode
    cp_bytes = CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)
    put_nonblocking("LongitudinalControl",
                    "1" if CP.openpilotLongitudinalControl else "0")

    CC = car.CarControl.new_message()
    AM = AlertManager()

    startup_alert = get_startup_alert(car_recognized, controller_available)
    AM.add(sm.frame, startup_alert, False)

    LoC = LongControl(CP, CI.compute_gb)
    VM = VehicleModel(CP)

    if CP.lateralTuning.which() == 'pid':
        LaC = LatControlPID(CP)
    elif CP.lateralTuning.which() == 'indi':
        LaC = LatControlINDI(CP)
    elif CP.lateralTuning.which() == 'lqr':
        LaC = LatControlLQR(CP)

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    mismatch_counter = 0
    can_error_counter = 0
    last_blinker_frame = 0
    saturated_count = 0
    events_prev = []

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['pathPlan'].sensorValid = True
    sm['pathPlan'].posenetValid = True
    sm['thermal'].freeSpace = 1.
    sm['dMonitoringState'].events = []
    sm['dMonitoringState'].awarenessStatus = 1.
    sm['dMonitoringState'].faceDetected = False

    # detect sound card presence
    sounds_available = not os.path.isfile('/EON') or (
        os.path.isdir('/proc/asound/card0')
        and open('/proc/asound/card0/state').read().strip() == 'ONLINE')

    # controlsd is driven by can recv, expected at 100Hz
    rk = Ratekeeper(100, print_delay_threshold=None)

    prof = Profiler(False)  # off by default

    while True:
        start_time = sec_since_boot()
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(
            CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
            params)
        prof.checkpoint("Sample")

        # Create alerts
        if not sm.alive['plan'] and sm.alive[
                'pathPlan']:  # only plan not being received: radar not communicating
            events.append(
                create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        elif not sm.all_alive_and_valid():
            events.append(
                create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not sm['pathPlan'].mpcSolutionValid:
            events.append(
                create_event('plannerError',
                             [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
            events.append(
                create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
        if not sm['pathPlan'].paramsValid:
            events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
        if not sm['pathPlan'].posenetValid:
            events.append(
                create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
        if not sm['plan'].radarValid:
            events.append(
                create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if sm['plan'].radarCanError:
            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]))
        if not sounds_available:
            events.append(
                create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
        if internet_needed:
            events.append(
                create_event('internetConnectivityNeeded',
                             [ET.NO_ENTRY, ET.PERMANENT]))
        if community_feature_disallowed:
            events.append(
                create_event('communityFeatureDisallowed', [ET.PERMANENT]))
        if read_only and not passive:
            events.append(create_event('carUnrecognized', [ET.PERMANENT]))
        if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
            events.append(
                create_event(
                    'relayMalfunction',
                    [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and sm[
                'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not read_only:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
          state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                        LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)

        prof.checkpoint("State Control")

        # Publish data
        CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
                                    actuators, v_cruise_kph, rk, AM, LaC, LoC,
                                    read_only, start_time, v_acc, a_acc,
                                    lac_log, events_prev, last_blinker_frame,
                                    is_ldw_enabled, can_error_counter)
        prof.checkpoint("Sent")

        rk.monitor_time()
        prof.display()
Exemple #24
0
def thermald_thread():
  health_timeout = int(1000 * 2.5 * DT_TRML)  # 2.5x the expected health frequency

  # now loop
  thermal_sock = messaging.pub_sock('thermal')
  health_sock = messaging.sub_sock('health', timeout=health_timeout)
  location_sock = messaging.sub_sock('gpsLocation')

  ignition = False
  fan_speed = 0
  count = 0

  off_ts = None
  started_ts = None
  started_seen = False
  thermal_status = ThermalStatus.green
  thermal_status_prev = ThermalStatus.green
  usb_power = True
  usb_power_prev = True
  current_branch = get_git_branch()

  network_type = NetworkType.none
  network_strength = NetworkStrength.unknown

  current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
  cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
  health_prev = None
  fw_version_match_prev = True
  current_update_alert = None
  time_valid_prev = True
  should_start_prev = False
  handle_fan = None
  is_uno = False
  has_relay = False

  params = Params()
  pm = PowerMonitoring()
  no_panda_cnt = 0

  while 1:
    health = messaging.recv_sock(health_sock, wait=True)
    location = messaging.recv_sock(location_sock)
    location = location.gpsLocation if location else None
    msg = read_thermal()

    if health is not None:
      usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client

      # If we lose connection to the panda, wait 5 seconds before going offroad
      if health.health.hwType == log.HealthData.HwType.unknown:
        no_panda_cnt += 1
        if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
          if ignition:
            cloudlog.error("Lost panda connection while onroad")
          ignition = False
      else:
        no_panda_cnt = 0
        ignition = health.health.ignitionLine or health.health.ignitionCan

      # Setup fan handler on first connect to panda
      if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown:
        is_uno = health.health.hwType == log.HealthData.HwType.uno
        has_relay = health.health.hwType in [log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno, log.HealthData.HwType.dos]

        if is_uno or not ANDROID:
          cloudlog.info("Setting up UNO fan handler")
          handle_fan = handle_fan_uno
        else:
          cloudlog.info("Setting up EON fan handler")
          setup_eon_fan()
          handle_fan = handle_fan_eon

      # Handle disconnect
      if health_prev is not None:
        if health.health.hwType == log.HealthData.HwType.unknown and \
          health_prev.health.hwType != log.HealthData.HwType.unknown:
          params.panda_disconnect()
      health_prev = health

    # get_network_type is an expensive call. update every 10s
    if (count % int(10. / DT_TRML)) == 0:
      try:
        network_type = get_network_type()
        network_strength = get_network_strength(network_type)
      except Exception:
        cloudlog.exception("Error getting network status")

    msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
    msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
    msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
    msg.thermal.networkType = network_type
    msg.thermal.networkStrength = network_strength
    msg.thermal.batteryPercent = get_battery_capacity()
    msg.thermal.batteryStatus = get_battery_status()
    msg.thermal.batteryCurrent = get_battery_current()
    msg.thermal.batteryVoltage = get_battery_voltage()
    msg.thermal.usbOnline = get_usb_present()

    # Fake battery levels on uno for frame
    if is_uno:
      msg.thermal.batteryPercent = 100
      msg.thermal.batteryStatus = "Charging"
      msg.thermal.bat = 0

    current_filter.update(msg.thermal.batteryCurrent / 1e6)

    # TODO: add car battery voltage check
    max_cpu_temp = cpu_temp_filter.update(
      max(msg.thermal.cpu0,
          msg.thermal.cpu1,
          msg.thermal.cpu2,
          msg.thermal.cpu3) / 10.0)

    max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.)
    bat_temp = msg.thermal.bat / 1000.

    if handle_fan is not None:
      fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition)
      msg.thermal.fanSpeed = fan_speed

    # If device is offroad we want to cool down before going onroad
    # since going onroad increases load and can make temps go over 107
    # We only do this if there is a relay that prevents the car from faulting
    if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and (started_ts is None) and max_cpu_temp > 70.0):
      # onroad not allowed
      thermal_status = ThermalStatus.danger
    elif max_comp_temp > 96.0 or bat_temp > 60.:
      # hysteresis between onroad not allowed and engage not allowed
      thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
    elif max_cpu_temp > 94.0:
      # hysteresis between engage not allowed and uploader not allowed
      thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red)
    elif max_cpu_temp > 80.0:
      # uploader not allowed
      thermal_status = ThermalStatus.yellow
    elif max_cpu_temp > 75.0:
      # hysteresis between uploader not allowed and all good
      thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow)
    else:
      # all good
      thermal_status = ThermalStatus.green

    # **** starting logic ****

    # Check for last update time and display alerts if needed
    now = datetime.datetime.utcnow()

    # show invalid date/time alert
    time_valid = now.year >= 2019
    if time_valid and not time_valid_prev:
      set_offroad_alert("Offroad_InvalidTime", False)
    if not time_valid and time_valid_prev:
      set_offroad_alert("Offroad_InvalidTime", True)
    time_valid_prev = time_valid

    # Show update prompt
    try:
      last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
    except (TypeError, ValueError):
      last_update = now
    dt = now - last_update

    update_failed_count = params.get("UpdateFailedCount")
    update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
    last_update_exception = params.get("LastUpdateException", encoding='utf8')

    if update_failed_count > 15 and last_update_exception is not None:
      if current_branch in ["release2", "dashcam"]:
        extra_text = "Ensure the software is correctly installed"
      else:
        extra_text = last_update_exception

      if current_update_alert != "update" + extra_text:
        current_update_alert = "update" + extra_text
        set_offroad_alert("Offroad_ConnectivityNeeded", False)
        set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
        set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text)
    elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
      if current_update_alert != "expired":
        current_update_alert = "expired"
        set_offroad_alert("Offroad_UpdateFailed", False)
        set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
        set_offroad_alert("Offroad_ConnectivityNeeded", True)
    elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
      remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
      if current_update_alert != "prompt" + remaining_time:
        current_update_alert = "prompt" + remaining_time
        set_offroad_alert("Offroad_UpdateFailed", False)
        set_offroad_alert("Offroad_ConnectivityNeeded", False)
        set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
    elif current_update_alert is not None:
      current_update_alert = None
      set_offroad_alert("Offroad_UpdateFailed", False)
      set_offroad_alert("Offroad_ConnectivityNeeded", False)
      set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)

    do_uninstall = params.get("DoUninstall") == b"1"
    accepted_terms = params.get("HasAcceptedTerms") == terms_version
    completed_training = params.get("CompletedTrainingVersion") == training_version

    panda_signature = params.get("PandaFirmware")
    fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE)   # don't show alert is no panda is connected (None)

    should_start = ignition

    # with 2% left, we killall, otherwise the phone will take a long time to boot
    should_start = should_start and msg.thermal.freeSpace > 0.02

    # confirm we have completed training and aren't uninstalling
    should_start = should_start and accepted_terms and completed_training and (not do_uninstall)

    # check for firmware mismatch
    should_start = should_start and fw_version_match

    # check if system time is valid
    should_start = should_start and time_valid

    # don't start while taking snapshot
    if not should_start_prev:
      is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
      is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
      should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)

    if fw_version_match and not fw_version_match_prev:
      set_offroad_alert("Offroad_PandaFirmwareMismatch", False)
    if not fw_version_match and fw_version_match_prev:
      set_offroad_alert("Offroad_PandaFirmwareMismatch", True)

    # if any CPU gets above 107 or the battery gets above 63, kill all processes
    # controls will warn with CPU above 95 or battery above 60
    if thermal_status >= ThermalStatus.danger:
      should_start = False
      if thermal_status_prev < ThermalStatus.danger:
        set_offroad_alert("Offroad_TemperatureTooHigh", True)
    else:
      if thermal_status_prev >= ThermalStatus.danger:
        set_offroad_alert("Offroad_TemperatureTooHigh", False)

    if should_start:
      if not should_start_prev:
        params.delete("IsOffroad")

      off_ts = None
      if started_ts is None:
        started_ts = sec_since_boot()
        started_seen = True
        os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
    else:
      if should_start_prev or (count == 0):
        put_nonblocking("IsOffroad", "1")

      started_ts = None
      if off_ts is None:
        off_ts = sec_since_boot()
        os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')

    # Offroad power monitoring
    pm.calculate(health)
    msg.thermal.offroadPowerUsage = pm.get_power_used()
    msg.thermal.carBatteryCapacity = pm.get_car_battery_capacity()

    # Check if we need to disable charging (handled by boardd)
    msg.thermal.chargingDisabled = pm.should_disable_charging(health, off_ts)

    # Check if we need to shut down
    if pm.should_shutdown(health, off_ts, started_seen, LEON):
      os.system('LD_LIBRARY_PATH="" svc power shutdown')

    msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90  # if current is positive, then battery is being discharged
    msg.thermal.started = started_ts is not None
    msg.thermal.startedTs = int(1e9*(started_ts or 0))

    msg.thermal.thermalStatus = thermal_status
    thermal_sock.send(msg.to_bytes())

    if usb_power_prev and not usb_power:
      set_offroad_alert("Offroad_ChargeDisabled", True)
    elif usb_power and not usb_power_prev:
      set_offroad_alert("Offroad_ChargeDisabled", False)

    thermal_status_prev = thermal_status
    usb_power_prev = usb_power
    fw_version_match_prev = fw_version_match
    should_start_prev = should_start

    # report to server once per minute
    if (count % int(60. / DT_TRML)) == 0:
      cloudlog.event("STATUS_PACKET",
                     count=count,
                     health=(health.to_dict() if health else None),
                     location=(location.to_dict() if location else None),
                     thermal=msg.to_dict())

    count += 1
Exemple #25
0
def main(sm=None, pm=None):
    if sm is None:
        sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
    if pm is None:
        pm = messaging.PubMaster(['liveParameters'])

    params_reader = Params()
    # wait for stats about the car to come in from controls
    cloudlog.info("paramsd is waiting for CarParams")
    CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
    cloudlog.info("paramsd got CarParams")

    params = params_reader.get("LiveParameters")

    # Check if car model matches
    if params is not None:
        params = json.loads(params)
        if params.get('carFingerprint', None) != CP.carFingerprint:
            cloudlog.info("Parameter learner found parameters for wrong car.")
            params = None

    if params is None:
        params = {
            'carFingerprint': CP.carFingerprint,
            'steerRatio': CP.steerRatio,
            'stiffnessFactor': 1.0,
            'angleOffsetAverage': 0.0,
        }
        cloudlog.info("Parameter learner resetting to default values")

    learner = ParamsLearner(CP, params['steerRatio'],
                            params['stiffnessFactor'],
                            math.radians(params['angleOffsetAverage']))

    i = 0
    while True:
        sm.update()

        for which, updated in sm.updated.items():
            if not updated:
                continue
            t = sm.logMonoTime[which] * 1e-9
            learner.handle_log(t, which, sm[which])

        # TODO: set valid to false when locationd stops sending
        # TODO: make sure controlsd knows when there is no gyro

        if sm.updated['carState']:
            msg = messaging.new_message('liveParameters')
            msg.logMonoTime = sm.logMonoTime['carState']

            msg.liveParameters.valid = True  # TODO: Check if learned values are sane
            msg.liveParameters.posenetValid = True
            msg.liveParameters.sensorValid = True

            x = learner.kf.x
            msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
            msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
            msg.liveParameters.angleOffsetAverage = math.degrees(
                x[States.ANGLE_OFFSET])
            msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(
                x[States.ANGLE_OFFSET_FAST])

            i += 1
            if i % 6000 == 0:  # once a minute
                params = {
                    'carFingerprint': CP.carFingerprint,
                    'steerRatio': msg.liveParameters.steerRatio,
                    'stiffnessFactor': msg.liveParameters.stiffnessFactor,
                    'angleOffsetAverage':
                    msg.liveParameters.angleOffsetAverage,
                }
                put_nonblocking("LiveParameters", json.dumps(params))

            # P = learner.kf.P
            # print()
            # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
            # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
            # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
            # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)

            pm.send('liveParameters', msg)
Exemple #26
0
def main(sm=None, pm=None):
    gc.disable()

    if sm is None:
        sm = messaging.SubMaster(['liveLocationKalman', 'carState'],
                                 poll=['liveLocationKalman'])
    if pm is None:
        pm = messaging.PubMaster(['liveParameters'])

    params_reader = Params()
    # wait for stats about the car to come in from controls
    cloudlog.info("paramsd is waiting for CarParams")
    CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
    cloudlog.info("paramsd got CarParams")

    min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio

    params = params_reader.get("LiveParameters")

    # Check if car model matches
    if params is not None:
        params = json.loads(params)
        if params.get('carFingerprint', None) != CP.carFingerprint:
            cloudlog.info("Parameter learner found parameters for wrong car.")
            params = None

    # Check if starting values are sane
    if params is not None:
        try:
            angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
            steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
            params_sane = angle_offset_sane and steer_ratio_sane
            if not params_sane:
                cloudlog.info(f"Invalid starting values found {params}")
                params = None
        except Exception as e:
            cloudlog.info(f"Error reading params {params}: {str(e)}")
            params = None

    # TODO: cache the params with the capnp struct
    if params is None:
        params = {
            'carFingerprint': CP.carFingerprint,
            'steerRatio': CP.steerRatio,
            'stiffnessFactor': 1.0,
            'angleOffsetAverageDeg': 0.0,
        }
        cloudlog.info("Parameter learner resetting to default values")

    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
    # Without a way to detect this we have to reset the stiffness every drive
    params['stiffnessFactor'] = 1.0

    learner = ParamsLearner(CP, params['steerRatio'],
                            params['stiffnessFactor'],
                            math.radians(params['angleOffsetAverageDeg']))

    while True:
        sm.update()

        for which, updated in sm.updated.items():
            if updated:
                t = sm.logMonoTime[which] * 1e-9
                learner.handle_log(t, which, sm[which])

        if sm.updated['liveLocationKalman']:
            x = learner.kf.x
            if not all(map(math.isfinite, x)):
                cloudlog.error(
                    "NaN in liveParameters estimate. Resetting to default values"
                )
                learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
                x = learner.kf.x

            msg = messaging.new_message('liveParameters')
            msg.logMonoTime = sm.logMonoTime['carState']

            msg.liveParameters.posenetValid = True
            msg.liveParameters.sensorValid = True
            msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
            msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
            msg.liveParameters.angleOffsetAverageDeg = math.degrees(
                x[States.ANGLE_OFFSET])
            msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees(
                x[States.ANGLE_OFFSET_FAST])
            msg.liveParameters.valid = all((
                abs(msg.liveParameters.angleOffsetAverageDeg) < 30.0,
                abs(msg.liveParameters.angleOffsetDeg) < 30.0,
                0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
                min_sr <= msg.liveParameters.steerRatio <= max_sr,
            ))

            if sm.frame % 1200 == 0:  # once a minute
                params = {
                    'carFingerprint':
                    CP.carFingerprint,
                    'steerRatio':
                    msg.liveParameters.steerRatio,
                    'stiffnessFactor':
                    msg.liveParameters.stiffnessFactor,
                    'angleOffsetAverageDeg':
                    msg.liveParameters.angleOffsetAverageDeg,
                }
                put_nonblocking("LiveParameters", json.dumps(params))

            pm.send('liveParameters', msg)
Exemple #27
0
def confd_thread():
    sm = messaging.SubMaster(['thermal'])
    pm = messaging.PubMaster(['dragonConf'])
    last_dp_msg = None
    last_modified = None
    update_params = False
    frame = 0
    locale = getprop("persist.sys.locale").rstrip('\n')

    last_autoshutdown = False
    last_sec = None
    autoshutdown_frame = 0

    last_charging_ctrl = False
    last_started = False

    dashcam_next_frame = 0
    dashcam_folder_exists = False
    dashcam_mkdir_retry = 0

    # thermal
    started = False
    free_space = 0.
    online = False
    battery_percent = 0.
    overheat = False

    while True:
        start_sec = sec_since_boot()
        if frame % 6 == 0:
            sm.update()
            if sm.updated['thermal']:
                started = sm['thermal'].started
                free_space = sm['thermal'].freeSpace
                online = sm['thermal'].usbOnline
                battery_percent = sm['thermal'].batteryPercent
                overheat = sm['thermal'].thermalStatus >= 2

        msg = messaging.new_message('dragonConf')
        if last_dp_msg is not None:
            msg.dragonConf = last_dp_msg
        '''
    ===================================================
    force update when start status changed
    ===================================================
    '''
        if last_started != started:
            update_params = True
        '''
    ===================================================
    we only need to check last_modified every 3 seconds
    if val is modified, we then proceed to read params
    ===================================================
    '''
        if not update_params and frame % 6 == 0:
            try:
                modified = params.get('dp_last_modified',
                                      encoding='utf8').rstrip('\x00')
            except AttributeError:
                modified = str(floor(time.time()))
            if last_modified != modified:
                update_params = True
            last_modified = modified
        '''
    ===================================================
    push param vals to message
    ===================================================
    '''
        if update_params:
            for conf in confs:
                conf_type = conf.get('conf_type')
                if 'param' in conf_type and 'struct' in conf_type:
                    update_this_conf = True

                    update_once = conf.get('update_once')
                    if update_once is not None and update_once is True and frame > 0:
                        update_this_conf = False

                    if frame > 0 and update_this_conf:
                        dependencies = conf.get('depends')
                        # if has dependency and the depend param val is not in depend_vals, we dont update that conf val
                        # this should reduce chance of reading all params
                        if dependencies is not None:
                            for dependency in dependencies:
                                if getattr(msg.dragonConf,
                                           get_struct_name(dependency['name'])
                                           ) not in dependency['vals']:
                                    update_this_conf = False

                    if update_this_conf:
                        val = params.get(conf['name'], encoding='utf8')
                        if val is not None:
                            val = val.rstrip('\x00')
                        struct_val = to_struct_val(conf['name'], val)
                        orig_val = struct_val
                        if conf.get('min') is not None:
                            struct_val = max(struct_val, conf.get('min'))
                        if conf.get('max') is not None:
                            struct_val = min(struct_val, conf.get('max'))
                        if orig_val != struct_val:
                            params.put(conf['name'], str(struct_val))
                        setattr(msg.dragonConf, get_struct_name(conf['name']),
                                struct_val)
        '''
    ===================================================
    push ip addr every 10 secs to message
    ===================================================
    '''
        if frame % 20 == 0:
            val = 'N/A'
            try:
                result = subprocess.check_output(["ifconfig", "wlan0"],
                                                 encoding='utf8')
                val = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
            except:
                pass
            msg.dragonConf.dpIpAddr = val
        '''
    ===================================================
    push is_updating status every 5 secs to message
    ===================================================
    '''
        if frame % 10 == 0:
            val = params.get('dp_is_updating', encoding='utf8').rstrip('\x00')
            setattr(msg.dragonConf, get_struct_name('dp_is_updating'),
                    to_struct_val('dp_is_updating', val))
        '''
    ===================================================
    push once
    ===================================================
    '''
        if frame == 0:
            setattr(msg.dragonConf, get_struct_name('dp_locale'), locale)
            put_nonblocking('dp_is_updating', '0')
        '''
    ===================================================
    we can have some logic here
    ===================================================
    '''
        if msg.dragonConf.dpAssistedLcMinMph > msg.dragonConf.dpAutoLcMinMph:
            put_nonblocking('dp_auto_lc_min_mph',
                            str(msg.dragonConf.dpAssistedLcMinMph))
            msg.dragonConf.dpAutoLcMinMph = msg.dragonConf.dpAssistedLcMinMph
        if msg.dragonConf.dpAtl:
            msg.dragonConf.dpAllowGas = True
            msg.dragonConf.dpDynamicFollow = 0
            msg.dragonConf.dpAccelProfile = 0
            msg.dragonConf.dpGearCheck = False
        if msg.dragonConf.dpAppWaze:
            msg.dragonConf.dpDrivingUi = False
        if not msg.dragonConf.dpDriverMonitor:
            msg.dragonConf.dpUiFace = False

        msg.dragonConf.dpThermalStarted = started
        msg.dragonConf.dpThermalOverheat = overheat
        '''
    ===================================================
    publish msg
    ===================================================
    '''
        last_dp_msg = msg.dragonConf
        update_params = False
        pm.send('dragonConf', msg)
        '''
    ===================================================
    hotspot on boot
    we do it after 30 secs just in case
    ===================================================
    '''
        if frame == 60 and params.get("dp_hotspot_on_boot") == b'1':
            os.system("service call wifi 37 i32 0 i32 1 &")
        '''
    ===================================================
    dashcam
    ===================================================    
    '''
        dashcam = msg.dragonConf.dpDashcam
        if frame % 2 == 0 and dashcam:
            while not dashcam_folder_exists and dashcam_mkdir_retry <= 5:
                try:
                    if not os.path.exists(DASHCAM_VIDEOS_PATH):
                        os.makedirs(DASHCAM_VIDEOS_PATH)
                    else:
                        dashcam_folder_exists = True
                except OSError:
                    pass
                dashcam_mkdir_retry += 1
            if dashcam_folder_exists:
                if started:
                    if frame >= dashcam_next_frame - 2:
                        now = datetime.datetime.now()
                        file_name = now.strftime("%Y-%m-%d_%H-%M-%S")
                        os.system(
                            "screenrecord --bit-rate %s --time-limit %s %s%s.mp4 &"
                            % (DASHCAM_BIT_RATES, DASHCAM_DURATION,
                               DASHCAM_VIDEOS_PATH, file_name))
                        dashcam_next_frame = frame + DASHCAM_DURATION * 2
                else:
                    dashcam_next_frame = 0

                if frame % 120 == 0 and ((free_space < DASHCAM_FREESPACE_LIMIT)
                                         or
                                         (get_used_spaces() > DASHCAM_KEPT)):
                    try:
                        files = [
                            f for f in sorted(os.listdir(DASHCAM_VIDEOS_PATH))
                            if os.path.isfile(DASHCAM_VIDEOS_PATH + f)
                        ]
                        os.system("rm -fr %s &" %
                                  (DASHCAM_VIDEOS_PATH + files[0]))
                    except (IndexError, FileNotFoundError, OSError):
                        pass
        '''
    ===================================================
    auto shutdown
    ===================================================
    '''
        autoshutdown = msg.dragonConf.dpAutoShutdown
        if frame % 20 == 0 and autoshutdown:
            sec = msg.dragonConf.dpAutoShutdownIn * 60 * 2
            if last_autoshutdown != autoshutdown or last_sec != sec or started or online:
                autoshutdown_frame = frame + sec
            if not started and not online and sec > 0 and frame >= autoshutdown_frame:
                os.system('LD_LIBRARY_PATH="" svc power shutdown')
            last_sec = sec
        last_autoshutdown = autoshutdown
        '''
    ===================================================
    battery ctrl every 30 secs
    PowerMonitor in thermald turns back on every mins
    so lets turn it off more frequent
    ===================================================
    '''
        charging_ctrl = msg.dragonConf.dpChargingCtrl
        if last_charging_ctrl != charging_ctrl:
            set_battery_charging(True)
        if charging_ctrl and frame % 60 == 0:
            if battery_percent >= msg.dragonConf.dpDischargingAt and get_battery_charging(
            ):
                set_battery_charging(False)
            elif battery_percent <= msg.dragonConf.dpChargingAt and not get_battery_charging(
            ):
                set_battery_charging(True)
        last_charging_ctrl = charging_ctrl
        '''
    ===================================================
    make it 2 hz
    ===================================================
    '''
        last_started = started
        frame += 1
        sleep = 0.5 - (sec_since_boot() - start_sec)
        if sleep > 0:
            time.sleep(sleep)
    def __init__(self, sm=None, pm=None, can_sock=None):
        gc.disable()
        set_realtime_priority(3)

        self.trace_log = trace1.Loger("controlsd")
        # Setup sockets
        self.pm = pm
        if self.pm is None:
            self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
                                           'carControl', 'carEvents', 'carParams'])

        self.sm = sm
        if self.sm is None:
            socks = [
                'thermal', 'health', 'model', 'liveCalibration',
                'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
            ]
            self.sm = messaging.SubMaster(socks,
                                          ignore_alive=['dMonitoringState'])

            #self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
            #                               'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])

        print(" start_Controls  messages...1")
        self.can_sock = can_sock
        if can_sock is None:
            can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
                                                 False) else 100
            self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

        print(" start_Controls  messages...2")
        # wait for one health and one CAN packet
        hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
        has_relay = hw_type in [HwType.blackPanda, HwType.uno]
        print("Waiting for CAN messages...")
        messaging.get_one_can(self.can_sock)

        self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
                                   has_relay)

        # read params
        params = Params()
        self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
        self.is_ldw_enabled = params.get("IsLdwEnabled",
                                         encoding='utf8') == "1"
        internet_needed = params.get("Offroad_ConnectivityNeeded",
                                     encoding='utf8') is not None
        community_feature_toggle = params.get("CommunityFeaturesToggle",
                                              encoding='utf8') == "1"
        openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
                                              encoding='utf8') == "1"
        passive = params.get("Passive", encoding='utf8') == "1" or \
                  internet_needed or not openpilot_enabled_toggle

        # detect sound card presence and ensure successful init
        sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
                                and open('/proc/asound/card0/state').read().strip() == 'ONLINE')

        car_recognized = self.CP.carName != 'mock'
        # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
        controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
        community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
        self.read_only = not car_recognized or not controller_available or \
                           self.CP.dashcamOnly or community_feature_disallowed
        if self.read_only:
            self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

        # Write CarParams for radard and boardd safety mode
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)
        put_nonblocking("LongitudinalControl",
                        "1" if self.CP.openpilotLongitudinalControl else "0")

        self.CC = car.CarControl.new_message()
        self.AM = AlertManager()
        self.events = Events()

        self.LoC = LongControl(self.CP, self.CI.compute_gb)
        self.VM = VehicleModel(self.CP)

        print('self.CP.lateralTuning.which()={}'.format(
            self.CP.lateralTuning.which()))
        if self.CP.lateralTuning.which() == 'pid':
            self.LaC = LatControlPID(self.CP)
        elif self.CP.lateralTuning.which() == 'indi':
            self.LaC = LatControlINDI(self.CP)
        elif self.CP.lateralTuning.which() == 'lqr':
            self.LaC = LatControlLQR(self.CP)

        self.state = State.disabled
        self.enabled = False
        self.active = False
        self.can_rcv_error = False
        self.soft_disable_timer = 0
        self.v_cruise_kph = 255
        self.v_cruise_kph_last = 0
        self.mismatch_counter = 0
        self.can_error_counter = 0
        self.consecutive_can_error_count = 0
        self.last_blinker_frame = 0
        self.saturated_count = 0
        self.events_prev = []
        self.current_alert_types = []

        self.sm['liveCalibration'].calStatus = Calibration.INVALID
        self.sm['thermal'].freeSpace = 1.
        self.sm['dMonitoringState'].events = []
        self.sm['dMonitoringState'].awarenessStatus = 1.
        self.sm['dMonitoringState'].faceDetected = False

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available, hw_type)

        if not sounds_available:
            self.events.add(EventName.soundsUnavailable, static=True)
        if internet_needed:
            self.events.add(EventName.internetConnectivityNeeded, static=True)
        if community_feature_disallowed:
            self.events.add(EventName.communityFeatureDisallowed, static=True)
        if self.read_only and not passive:
            self.events.add(EventName.carUnrecognized, static=True)
        # if hw_type == HwType.whitePanda:
        #   self.events.add(EventName.whitePandaUnsupported, static=True)

        uname = subprocess.check_output(["uname", "-v"],
                                        encoding='utf8').strip()
        if uname == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020":
            self.events.add(EventName.neosUpdateRequired, static=True)

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default

        self.hyundai_lkas = self.read_only  #read_only
        self.init_flag = True
Exemple #29
0
  def __init__(self, sm=None, pm=None, can_sock=None):
    config_realtime_process(3, Priority.CTRL_HIGH)

    # Setup sockets
    self.pm = pm
    if self.pm is None:
      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
                                     'carControl', 'carEvents', 'carParams'])

    self.sm = sm
    if self.sm is None:
      self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration',
                                     'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])

    self.can_sock = can_sock
    if can_sock is None:
      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

    # wait for one health and one CAN packet
    hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
    has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
    print("Waiting for CAN messages...")
    get_one_can(self.can_sock)

    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)

    # read params
    params = Params()
    self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
    self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
    internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1")
    community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
    openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
    passive = params.get("Passive", encoding='utf8') == "1" or \
              internet_needed or not openpilot_enabled_toggle

    # detect sound card presence and ensure successful init
    sounds_available = HARDWARE.get_sound_card_online()

    car_recognized = self.CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
    controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
    community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
    self.read_only = not car_recognized or not controller_available or \
                       self.CP.dashcamOnly or community_feature_disallowed
    if self.read_only:
      self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

    # Write CarParams for radard and boardd safety mode
    cp_bytes = self.CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)

    self.CC = car.CarControl.new_message()
    self.AM = AlertManager()
    self.events = Events()

    self.LoC = LongControl(self.CP, self.CI.compute_gb)
    self.VM = VehicleModel(self.CP)

    if self.CP.lateralTuning.which() == 'pid':
      self.LaC = LatControlPID(self.CP)
    elif self.CP.lateralTuning.which() == 'indi':
      self.LaC = LatControlINDI(self.CP)
    elif self.CP.lateralTuning.which() == 'lqr':
      self.LaC = LatControlLQR(self.CP)

    self.state = State.disabled
    self.enabled = False
    self.active = False
    self.can_rcv_error = False
    self.soft_disable_timer = 0
    self.v_cruise_kph = 255
    self.v_cruise_kph_last = 0
    self.mismatch_counter = 0
    self.can_error_counter = 0
    self.last_blinker_frame = 0
    self.saturated_count = 0
    self.distance_traveled = 0
    self.last_functional_fan_frame = 0
    self.events_prev = []
    self.current_alert_types = [ET.PERMANENT]

    self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
    self.sm['thermal'].freeSpace = 1.
    self.sm['dMonitoringState'].events = []
    self.sm['dMonitoringState'].awarenessStatus = 1.
    self.sm['dMonitoringState'].faceDetected = False

    self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)

    if not sounds_available:
      self.events.add(EventName.soundsUnavailable, static=True)
    if internet_needed:
      self.events.add(EventName.internetConnectivityNeeded, static=True)
    if community_feature_disallowed:
      self.events.add(EventName.communityFeatureDisallowed, static=True)
    if not car_recognized:
      self.events.add(EventName.carUnrecognized, static=True)
    if hw_type == HwType.whitePanda:
      self.events.add(EventName.whitePandaUnsupported, static=True)

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default
Exemple #30
0
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

        # Setup sockets
        self.pm = pm
        if self.pm is None:
            self.pm = messaging.PubMaster([
                'sendcan', 'controlsState', 'carState', 'carControl',
                'carEvents', 'carParams'
            ])

        self.sm = sm
        if self.sm is None:
            ignore = ['driverCameraState', 'managerState'
                      ] if SIMULATION else None
            self.sm = messaging.SubMaster([
                'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
                'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
                'liveLocationKalman', 'roadCameraState', 'driverCameraState',
                'managerState', 'liveParameters', 'radarState'
            ],
                                          ignore_alive=ignore)

        self.can_sock = can_sock
        if can_sock is None:
            can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
                                                 False) else 100
            self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

        # wait for one pandaState and one CAN packet
        print("Waiting for CAN messages...")
        get_one_can(self.can_sock)

        self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])

        # read params
        params = Params()
        self.is_metric = params.get_bool("IsMetric")
        self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
        community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
        openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
        passive = params.get_bool("Passive") or not openpilot_enabled_toggle

        # detect sound card presence and ensure successful init
        sounds_available = HARDWARE.get_sound_card_online()

        car_recognized = self.CP.carName != 'mock'
        # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
        controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
        community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
        self.read_only = not car_recognized or not controller_available or \
                           self.CP.dashcamOnly or community_feature_disallowed
        if self.read_only:
            self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

        # Write CarParams for radard and boardd safety mode
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)

        self.CC = car.CarControl.new_message()
        self.AM = AlertManager()
        self.events = Events()

        self.LoC = LongControl(self.CP, self.CI.compute_gb)
        self.VM = VehicleModel(self.CP)

        if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
            self.LaC = LatControlAngle(self.CP)
        elif self.CP.lateralTuning.which() == 'pid':
            self.LaC = LatControlPID(self.CP)
        elif self.CP.lateralTuning.which() == 'indi':
            self.LaC = LatControlINDI(self.CP)
        elif self.CP.lateralTuning.which() == 'lqr':
            self.LaC = LatControlLQR(self.CP)

        self.state = State.disabled
        self.enabled = False
        self.active = False
        self.can_rcv_error = False
        self.soft_disable_timer = 0
        self.v_cruise_kph = 255
        self.v_cruise_kph_last = 0
        self.mismatch_counter = 0
        self.can_error_counter = 0
        self.last_blinker_frame = 0
        self.saturated_count = 0
        self.distance_traveled = 0
        self.last_functional_fan_frame = 0
        self.events_prev = []
        self.current_alert_types = [ET.PERMANENT]
        self.logged_comm_issue = False

        self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
        self.sm['deviceState'].freeSpacePercent = 100
        self.sm['driverMonitoringState'].events = []
        self.sm['driverMonitoringState'].awarenessStatus = 1.
        self.sm['driverMonitoringState'].faceDetected = False
        self.sm['liveParameters'].valid = True

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available)

        if not sounds_available:
            self.events.add(EventName.soundsUnavailable, static=True)
        if community_feature_disallowed:
            self.events.add(EventName.communityFeatureDisallowed, static=True)
        if not car_recognized:
            self.events.add(EventName.carUnrecognized, static=True)
        elif self.read_only:
            self.events.add(EventName.dashcamMode, static=True)

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default