merge master

selfdrive/car/conversions.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+class Conversions:
+  # Speed
+  MPH_TO_KPH = 1.609344
+  KPH_TO_MPH = 1. / MPH_TO_KPH
+  MS_TO_KPH = 3.6
+  KPH_TO_MS = 1. / MS_TO_KPH
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
+  MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+
+  # Angle
+  DEG_TO_RAD = np.pi / 180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
+
+  # Mass
+  LB_TO_KG = 0.453592

selfdrive/car/filter_simple.py

@@ -0,0 +1,18 @@
+class FirstOrderFilter:
+  # first order filter
+  def __init__(self, x0, rc, dt, initialized=True):
+    self.x = x0
+    self.dt = dt
+    self.update_alpha(rc)
+    self.initialized = initialized
+
+  def update_alpha(self, rc):
+    self.alpha = self.dt / (rc + self.dt)
+
+  def update(self, x):
+    if self.initialized:
+      self.x = (1. - self.alpha) * self.x + self.alpha * x
+    else:
+      self.initialized = True
+      self.x = x
+    return self.x

selfdrive/car/helpers.py

@@ -0,0 +1,22 @@
+def clip(x, lo, hi):
+  return max(lo, min(hi, x))
+
+
+def interp(x, xp, fp):
+  N = len(xp)
+
+  def get_interp(xv):
+    hi = 0
+    while hi < N and xv > xp[hi]:
+      hi += 1
+    low = hi - 1
+    return fp[-1] if hi == N and xv > xp[low] else (
+      fp[0] if hi == 0 else
+      (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
+
+  return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x)
+
+
+def mean(x):
+  return sum(x) / len(x)
+

selfdrive/pandad/panda_safety.cc

@@ -0,0 +1,79 @@
+#include "selfdrive/pandad/pandad.h"
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+
+void PandaSafety::configureSafetyMode() {
+  bool is_onroad = params_.getBool("IsOnroad");
+
+  if (is_onroad && !safety_configured_) {
+    updateMultiplexingMode();
+
+    auto car_params = fetchCarParams();
+    if (!car_params.empty()) {
+      LOGW("got %lu bytes CarParams", car_params.size());
+      setSafetyMode(car_params);
+      safety_configured_ = true;
+    }
+  } else if (!is_onroad) {
+    initialized_ = false;
+    safety_configured_ = false;
+  }
+}
+
+void PandaSafety::updateMultiplexingMode() {
+  // Initialize to ELM327 without OBD multiplexing for initial fingerprinting
+  if (!initialized_) {
+    prev_obd_multiplexing_ = false;
+    for (int i = 0; i < pandas_.size(); ++i) {
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
+    }
+    initialized_ = true;
+  }
+
+  // Switch between multiplexing modes based on the OBD multiplexing request
+  bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled");
+  if (obd_multiplexing_requested != prev_obd_multiplexing_) {
+    for (int i = 0; i < pandas_.size(); ++i) {
+      const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
+    }
+    prev_obd_multiplexing_ = obd_multiplexing_requested;
+    params_.putBool("ObdMultiplexingChanged", true);
+  }
+}
+
+std::string PandaSafety::fetchCarParams() {
+  if (!params_.getBool("FirmwareQueryDone")) {
+    return {};
+  }
+  LOGW("Finished FW query");
+
+  LOGW("Waiting for params to set safety model");
+  if (!params_.getBool("ControlsReady")) {
+    return {};
+  }
+  return params_.get("CarParams");
+}
+
+void PandaSafety::setSafetyMode(const std::string &params_string) {
+  AlignedBuffer aligned_buf;
+  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size()));
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  auto safety_configs = car_params.getSafetyConfigs();
+  uint16_t alternative_experience = car_params.getAlternativeExperience();
+
+  for (int i = 0; i < pandas_.size(); ++i) {
+    // Default to SILENT safety model if not specified
+    cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT;
+    uint16_t safety_param = 0U;
+    if (i < safety_configs.size()) {
+      safety_model = safety_configs[i].getSafetyModel();
+      safety_param = safety_configs[i].getSafetyParam();
+    }
+
+    LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
+    pandas_[i]->set_alternative_experience(alternative_experience);
+    pandas_[i]->set_safety_model(safety_model, safety_param);
+  }
+}