openpilot v0.7.2 release

selfdrive/locationd/calibrationd.py

@@ -38,8 +38,8 @@ def is_calibration_valid(vp):
 def sanity_clip(vp):
   if np.isnan(vp).any():
     vp = VP_INIT
-  return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
-          np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)]
+  return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
+                   np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)])
 
 
 def intrinsics_from_vp(vp):
@@ -96,6 +96,7 @@ class Calibrator():
       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
@@ -103,8 +104,7 @@ class Calibrator():
         self.block_idx += 1
         self.valid_blocks = max(self.block_idx, self.valid_blocks)
         self.block_idx = self.block_idx % INPUTS_WANTED
-      raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
-      self.vp = sanity_clip(raw_vp)
+      self.vp = np.mean(self.vps[:max(1, 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):

selfdrive/locationd/locationd_yawrate.cc

@@ -10,7 +10,7 @@
 #include "locationd_yawrate.h"
 
 
-void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
+void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
   double dt = current_time - prev_update_time;
 
   if (dt < 0) {
@@ -24,7 +24,7 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
 
   double y = meas - C * x;
   double S = R + C * P * C.transpose();
-  Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
+  Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
   x = x + K * y;
   P = (I - K * C) * P;
 }
@@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
 }
 
 void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
-  double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
+  double R = pow(5 * camera_odometry.getRotStd()[2], 2);
   double meas = camera_odometry.getRot()[2];
   update_state(C_posenet, R, current_time, meas);
 
@@ -57,34 +57,27 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
 
 
 Localizer::Localizer() {
-  // States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
+  // States: [yaw rate, gyro bias]
   A <<
-    1, 1, 0, 0,
-    0, 1, 0, 0,
-    0, 0, 1, 1,
-    0, 0, 0, 1;
-  I <<
-    1, 0, 0, 0,
-    0, 1, 0, 0,
-    0, 0, 1, 0,
-    0, 0, 0, 1;
+    1, 0,
+    0, 1;
 
   Q <<
-    0, 0, 0, 0,
-    0, pow(0.1, 2.0), 0, 0,
-    0, 0, 0, 0,
-    0, 0, pow(0.005 / 100.0, 2.0), 0;
+    pow(.1, 2.0), 0,
+    0, pow(0.05/ 100.0, 2.0),
   P <<
-    pow(100.0, 2.0), 0, 0, 0,
-    0, pow(100.0, 2.0), 0, 0,
-    0, 0, pow(100.0, 2.0), 0,
-    0, 0, 0, pow(100.0, 2.0);
+    pow(10000.0, 2.0), 0,
+    0, pow(10000.0, 2.0);
 
-  C_posenet << 1, 0, 0, 0;
-  C_gyro << 1, 0, 1, 0;
-  x << 0, 0, 0, 0;
+  I <<
+    1, 0,
+    0, 1;
 
-  R_gyro = pow(0.25, 2.0);
+  C_posenet << 1, 0;
+  C_gyro << 1, 1;
+  x << 0, 0;
+
+  R_gyro = pow(0.025, 2.0);
 }
 
 void Localizer::handle_log(cereal::Event::Reader event) {
@@ -133,7 +126,7 @@ extern "C" {
   }
   double localizer_get_bias(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
-    return loc->x[2];
+    return loc->x[1];
   }
 
   double * localizer_get_state(void * localizer) {

selfdrive/locationd/locationd_yawrate.h

@@ -7,22 +7,22 @@
 
 class Localizer
 {
-  Eigen::Matrix4d A;
-  Eigen::Matrix4d I;
-  Eigen::Matrix4d Q;
-  Eigen::Matrix<double, 1, 4> C_posenet;
-  Eigen::Matrix<double, 1, 4> C_gyro;
+  Eigen::Matrix2d A;
+  Eigen::Matrix2d I;
+  Eigen::Matrix2d Q;
+  Eigen::Matrix<double, 1, 2> C_posenet;
+  Eigen::Matrix<double, 1, 2> C_gyro;
 
   double R_gyro;
 
-  void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
+  void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
   void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
   void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
   void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
 
 public:
-  Eigen::Vector4d x;
-  Eigen::Matrix4d P;
+  Eigen::Vector2d x;
+  Eigen::Matrix2d P;
   double steering_angle = 0;
   double car_speed = 0;
   double posenet_speed = 0;

selfdrive/locationd/paramsd.cc

@@ -20,6 +20,7 @@
 #include "locationd_yawrate.h"
 #include "params_learner.h"
 
+#include "common/util.h"
 
 void sigpipe_handler(int sig) {
   LOGE("SIGPIPE received");
@@ -141,7 +142,7 @@ int main(int argc, char *argv[]) {
         auto live_params = event.initLiveParameters();
         live_params.setValid(valid);
         live_params.setYawRate(localizer.x[0]);
-        live_params.setGyroBias(localizer.x[2]);
+        live_params.setGyroBias(localizer.x[1]);
         live_params.setSensorValid(sensor_data_age < 5.0);
         live_params.setAngleOffset(angle_offset_degrees);
         live_params.setAngleOffsetAverage(angle_offset_average_degrees);

selfdrive/locationd/test/ublox.py

@@ -475,6 +475,8 @@ msg_types = {
   UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
   (CLASS_RXM, MSG_RXM_ALM):
   UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
+  (CLASS_CFG, MSG_CFG_ANT):
+  UBloxDescriptor('CFG_ANT', '<HH', ['flags', 'pins']),
   (CLASS_CFG, MSG_CFG_ODO):
   UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
     'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
@@ -494,9 +496,9 @@ msg_types = {
     'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
   ]),
   (CLASS_MON, MSG_MON_HW):
-  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
+  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB17BHIII', [
     'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
-    'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
+    'flags', 'reserved1', 'usedMask', 'VP[17]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
     'pullL'
   ]),
   (CLASS_MON, MSG_MON_HW2):
@@ -827,7 +829,10 @@ class UBlox:
     if not self.read_only:
       if self.use_sendrecv:
         return self.dev.send(buf)
-      return self.dev.write(buf)
+      if type(buf) == str:
+        return self.dev.write(str.encode(buf))
+      else:
+        return self.dev.write(buf)
 
   def read(self, n):
     '''read some bytes'''
@@ -973,7 +978,7 @@ class UBlox:
     payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
     self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
 
-  def configure_poll(self, msg_class, msg_id, payload=''):
+  def configure_poll(self, msg_class, msg_id, payload=b''):
     '''poll a configuration message'''
     self.send_message(msg_class, msg_id, payload)
 

selfdrive/locationd/test/ubloxd.py

@@ -72,10 +72,11 @@ def configure_ublox(dev):
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
 
-  # Configure RAW and PVT messages to be sent every solution cycle
+  # Configure RAW, PVT and HW messages to be sent every solution cycle
   dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
   dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
   dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
+  dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
 
 
 
@@ -222,6 +223,17 @@ def gen_raw(msg):
                 'measurements': measurements_parsed}}
   return log.Event.new_message(ubloxGnss=raw_meas)
 
+def gen_hw_status(msg):
+  msg_data = msg.unpack()[0]
+  ublox_hw_status = {'hwStatus': {
+    'noisePerMS': msg_data['noisePerMS'],
+    'agcCnt': msg_data['agcCnt'],
+    'aStatus': msg_data['aStatus'],
+    'aPower': msg_data['aPower'],
+    'jamInd': msg_data['jamInd']
+  }}
+  return log.Event.new_message(ubloxGnss=ublox_hw_status)
+
 def init_reader():
   port_counter = 0
   while True:
@@ -252,9 +264,12 @@ def handle_msg(dev, msg, nav_frame_buffer):
       if nav is not None:
         nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
         ubloxGnss.send(nav.to_bytes())
-
+    elif msg.name() == 'MON_HW':
+      hw = gen_hw_status(msg)
+      hw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      ubloxGnss.send(hw.to_bytes())
     else:
-      print("UNKNNOWN MESSAGE:", msg.name())
+      print("UNKNOWN MESSAGE:", msg.name())
   except ublox.UBloxError as e:
     print(e)
 

selfdrive/locationd/ublox_msg.cc

@@ -347,6 +347,22 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
   return kj::Array<capnp::word>();
 }
 
+kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
+  mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+
+  capnp::MallocMessageBuilder msg_builder;
+  cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto gnss = event.initUbloxGnss();
+  auto hwStatus = gnss.initHwStatus();
+  hwStatus.setNoisePerMS(msg->noisePerMS);
+  hwStatus.setAgcCnt(msg->agcCnt);
+  hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus);
+  hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower);
+  hwStatus.setJamInd(msg->jamInd);
+  return capnp::messageToFlatArray(msg_builder);
+}
+
 bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
   int needed = needed_bytes();
   if(needed > 0) {

selfdrive/locationd/ublox_msg.h

@@ -85,6 +85,27 @@ typedef struct __attribute__((packed)) {
   uint32_t dwrd;
 } rxm_sfrbx_msg_extra;
 
+// MON_HW
+typedef struct __attribute__((packed)) {
+  uint32_t pinSel;
+  uint32_t pinBank;
+  uint32_t pinDir;
+  uint32_t pinVal;
+  uint16_t noisePerMS;
+  uint16_t agcCnt;
+  uint8_t aStatus;
+  uint8_t aPower;
+  uint8_t flags;
+  uint8_t reserved1;
+  uint32_t usedMask;
+  uint8_t VP[17];
+  uint8_t jamInd;
+  uint8_t reserved2[2];
+  uint32_t pinIrq;
+  uint32_t pullH;
+  uint32_t pullL;
+} mon_hw_msg;
+
 namespace ublox {
   // protocol constants
   const uint8_t PREAMBLE1 = 0xb5;
@@ -93,6 +114,7 @@ namespace ublox {
   // message classes
   const uint8_t CLASS_NAV = 0x01;
   const uint8_t CLASS_RXM = 0x02;
+  const uint8_t CLASS_MON = 0x0A;
 
   // NAV messages
   const uint8_t MSG_NAV_PVT = 0x7;
@@ -101,6 +123,9 @@ namespace ublox {
   const uint8_t MSG_RXM_RAW = 0x15;
   const uint8_t MSG_RXM_SFRBX = 0x13;
 
+  // MON messages
+  const uint8_t MSG_MON_HW = 0x09;
+
   const int UBLOX_HEADER_SIZE = 6;
   const int UBLOX_CHECKSUM_SIZE = 2;
   const int UBLOX_MAX_MSG_SIZE = 65536;
@@ -113,6 +138,7 @@ namespace ublox {
       UbloxMsgParser();
       kj::Array<capnp::word> gen_solution();
       kj::Array<capnp::word> gen_raw();
+      kj::Array<capnp::word> gen_mon_hw();
 
       kj::Array<capnp::word> gen_nav_data();
       bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);

selfdrive/locationd/ubloxd_main.cc

@@ -19,6 +19,7 @@
 #include <capnp/serialize.h>
 #include "cereal/gen/cpp/log.capnp.h"
 
+#include "common/util.h"
 #include "common/params.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
@@ -96,6 +97,17 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
             }
           } else
             LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
+        } else if(parser.msg_class() == CLASS_MON) {
+          if(parser.msg_id() == MSG_MON_HW) {
+            //LOGD("MSG_MON_HW");
+            auto words = parser.gen_mon_hw();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              send_func(ubloxGnss, bytes.begin(), bytes.size());
+            }
+          } else {
+            LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
+          }
         } else
           LOGW("Unknown msg class: 0x%02X", parser.msg_class());
         parser.reset();