openpilot v0.8.2 release

selfdrive/locationd/SConscript

@@ -2,14 +2,7 @@ Import('env', 'common', 'cereal', 'messaging')
 
 loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread']
 
-env.Program("ubloxd", [
-  "ubloxd.cc",
-  "ublox_msg.cc",
-  "ubloxd_main.cc"],
-  LIBS=loc_libs)
+env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs)
 
-env.Program("ubloxd_test", [
-  "ubloxd_test.cc",
-  "ublox_msg.cc",
-  "ubloxd_main.cc"],
-  LIBS=loc_libs)
+if GetOption("test"):
+  env.Program("ubloxd_test", ["ubloxd_test.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs)

selfdrive/locationd/locationd.py

@@ -1,8 +1,10 @@
 #!/usr/bin/env python3
+import json
 import numpy as np
 import sympy as sp
 import cereal.messaging as messaging
 from cereal import log
+from common.params import Params
 import common.transformations.coordinates as coord
 from common.transformations.orientation import ecef_euler_from_ned, \
                                                euler_from_quat, \
@@ -194,7 +196,7 @@ class Localizer():
 
     orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
     orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
-    orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
+    orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)])
     orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
     initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
     if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
@@ -238,6 +240,7 @@ class Localizer():
   def handle_sensors(self, current_time, log):
     # TODO does not yet account for double sensor readings in the log
     for sensor_reading in log:
+      sensor_time = 1e-9 * sensor_reading.timestamp
       # TODO: handle messages from two IMUs at the same time
       if sensor_reading.source == SensorSource.lsm6ds3:
         continue
@@ -247,7 +250,7 @@ class Localizer():
         self.gyro_counter += 1
         if self.gyro_counter % SENSOR_DECIMATION == 0:
           v = sensor_reading.gyroUncalibrated.v
-          self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
+          self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
 
       # Accelerometer
       if sensor_reading.sensor == 1 and sensor_reading.type == 1:
@@ -258,7 +261,7 @@ class Localizer():
         self.acc_counter += 1
         if self.acc_counter % SENSOR_DECIMATION == 0:
           v = sensor_reading.acceleration.v
-          self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
+          self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
 
   def handle_live_calib(self, current_time, log):
     if len(log.rpyCalib):
@@ -295,6 +298,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
   if pm is None:
     pm = messaging.PubMaster(['liveLocationKalman'])
 
+  params = Params()
   localizer = Localizer(disabled_logs=disabled_logs)
 
   while True:
@@ -327,6 +331,14 @@ def locationd_thread(sm, pm, disabled_logs=None):
       msg.liveLocationKalman.gpsOK = gps_age < 1.0
       pm.send('liveLocationKalman', msg)
 
+      if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK:  # once a minute
+        location = {
+          'latitude': msg.liveLocationKalman.positionGeodetic.value[0],
+          'longitude': msg.liveLocationKalman.positionGeodetic.value[1],
+          'altitude': msg.liveLocationKalman.positionGeodetic.value[2],
+        }
+        params.put("LastGPSPosition", json.dumps(location))
+
 
 def main(sm=None, pm=None):
   locationd_thread(sm, pm)

selfdrive/locationd/models/live_kf.py

@@ -57,7 +57,7 @@ class LiveKalman():
 
   # process noise
   Q = np.diag([0.03**2, 0.03**2, 0.03**2,
-               0.001**2, 0.001*2, 0.001**2,
+               0.001**2, 0.001**2, 0.001**2,
                0.01**2, 0.01**2, 0.01**2,
                0.1**2, 0.1**2, 0.1**2,
                (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,

selfdrive/locationd/paramsd.py

@@ -48,7 +48,7 @@ class ParamsLearner:
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
 
     elif which == 'carState':
-      self.steering_angle = msg.steeringAngle
+      self.steering_angle = msg.steeringAngleDeg
       self.steering_pressed = msg.steeringPressed
       self.speed = msg.vEgo
 
@@ -56,7 +56,7 @@ class ParamsLearner:
       self.active = self.speed > 5 and in_linear_region
 
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]]))
         self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
 
     if not self.active:
@@ -88,18 +88,23 @@ def main(sm=None, pm=None):
       cloudlog.info("Parameter learner found parameters for wrong car.")
       params = None
 
-  if (params is not None) and not all((
-      abs(params['angleOffsetAverage']) < 10.0,
-      min_sr <= params['steerRatio'] <= max_sr)):
-    cloudlog.info(f"Invalid starting values found {params}")
+  try:
+    if params is not None and not all((
+        abs(params.get('angleOffsetAverageDeg')) < 10.0,
+        min_sr <= params['steerRatio'] <= max_sr)):
+      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,
-      'angleOffsetAverage': 0.0,
+      'angleOffsetAverageDeg': 0.0,
     }
     cloudlog.info("Parameter learner resetting to default values")
 
@@ -107,7 +112,7 @@ def main(sm=None, pm=None):
   # 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['angleOffsetAverage']))
+  learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
 
   while True:
     sm.update()
@@ -127,11 +132,11 @@ def main(sm=None, pm=None):
       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])
+      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.angleOffsetAverage) < 10.0,
-        abs(msg.liveParameters.angleOffset) < 10.0,
+        abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0,
+        abs(msg.liveParameters.angleOffsetDeg) < 10.0,
         0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
         min_sr <= msg.liveParameters.steerRatio <= max_sr,
       ))
@@ -141,7 +146,7 @@ def main(sm=None, pm=None):
           'carFingerprint': CP.carFingerprint,
           'steerRatio': msg.liveParameters.steerRatio,
           'stiffnessFactor': msg.liveParameters.stiffnessFactor,
-          'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
+          'angleOffsetAverageDeg': msg.liveParameters.angleOffsetAverageDeg,
         }
         put_nonblocking("LiveParameters", json.dumps(params))
 

selfdrive/locationd/ublox_msg.cc

@@ -1,20 +1,11 @@
 #include <stdio.h>
 #include <stdlib.h>
-#include <signal.h>
 #include <unistd.h>
-#include <sched.h>
-#include <sys/time.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <assert.h>
 #include <math.h>
 #include <ctime>
 #include <chrono>
-#include <algorithm>
 
-#include "common/params.h"
 #include "common/swaglog.h"
-#include "common/timing.h"
 
 #include "ublox_msg.h"
 
@@ -173,8 +164,7 @@ inline bool UbloxMsgParser::valid_cheksum() {
 
 inline bool UbloxMsgParser::valid() {
   return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE &&
-    needed_bytes() == 0 &&
-    valid_cheksum();
+         needed_bytes() == 0 && valid_cheksum();
 }
 
 inline bool UbloxMsgParser::valid_so_far() {
@@ -186,8 +176,9 @@ inline bool UbloxMsgParser::valid_so_far() {
     //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]);
     return false;
   }
-  if(needed_bytes() == 0 && !valid())
+  if(needed_bytes() == 0 && !valid()) {
     return false;
+  }
   return true;
 }
 
@@ -201,7 +192,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
   gpsLoc.setLongitude(msg->lon * 1e-07);
   gpsLoc.setAltitude(msg->height * 1e-03);
   gpsLoc.setSpeed(msg->gSpeed * 1e-03);
-  gpsLoc.setBearing(msg->headMot * 1e-5);
+  gpsLoc.setBearingDeg(msg->headMot * 1e-5);
   gpsLoc.setAccuracy(msg->hAcc * 1e-03);
   std::tm timeinfo = std::tm();
   timeinfo.tm_year = msg->year - 1900;
@@ -216,12 +207,12 @@ kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
   gpsLoc.setVNED(f);
   gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
   gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
-  gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05);
+  gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05);
   return capnp::messageToFlatArray(msg_builder);
 }
 
 inline bool bit_to_bool(uint8_t val, int shifts) {
-  return (val & (1 << shifts)) ? true : false;
+  return (bool)(val & (1 << shifts));
 }
 
 kj::Array<capnp::word> UbloxMsgParser::gen_raw() {
@@ -282,13 +273,15 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
     for(int i = 0; i < msg->numWords;i++)
       words.push_back(measurements[i].dwrd);
 
-    if(subframeId == 1) {
-      nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map();
-      nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
-    } else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end())
-      nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
-    if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) {
-      EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]);
+    subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid];
+    if (subframeId == 1) {
+      map = subframes_map();
+      map[subframeId] = words;
+    } else if (map.find(subframeId-1) != map.end()) {
+      map[subframeId] = words;
+    }
+    if(map.size() == 5) {
+      EphemerisData ephem_data(msg->svid, map);
       MessageBuilder msg_builder;
       auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
       eph.setSvId(ephem_data.svId);
@@ -342,10 +335,44 @@ kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
   return capnp::messageToFlatArray(msg_builder);
 }
 
+kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2() {
+  mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+
+  MessageBuilder msg_builder;
+  auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2();
+  hwStatus.setOfsI(msg->ofsI);
+  hwStatus.setMagI(msg->magI);
+  hwStatus.setOfsQ(msg->ofsQ);
+  hwStatus.setMagQ(msg->magQ);
+
+  switch (msg->cfgSource) {
+    case 114:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM);
+      break;
+    case 111:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
+      break;
+    case 112:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
+      break;
+    case 102:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
+      break;
+    default:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED);
+      break;
+  }
+
+  hwStatus.setLowLevCfg(msg->lowLevCfg);
+  hwStatus.setPostStatus(msg->postStatus);
+
+  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) {
-    bytes_consumed = min((size_t)needed, incoming_data_len );
+    bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
     // Add data to buffer
     memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
     bytes_in_parse_buf += bytes_consumed;

selfdrive/locationd/ublox_msg.h

@@ -3,8 +3,6 @@
 #include <stdint.h>
 #include "messaging.hpp"
 
-#define min(x, y) ((x) <= (y) ? (x) : (y))
-
 // NAV_PVT
 typedef struct __attribute__((packed)) {
   uint32_t iTOW;
@@ -68,6 +66,7 @@ typedef struct __attribute__((packed)) {
   int8_t trkStat;
   int8_t reserved3;
 } rxm_raw_msg_extra;
+
 // RXM_SFRBX
 typedef struct __attribute__((packed)) {
   int8_t gnssId;
@@ -106,6 +105,20 @@ typedef struct __attribute__((packed)) {
   uint32_t pullL;
 } mon_hw_msg;
 
+// MON_HW2
+typedef struct __attribute__((packed)) {
+  int8_t ofsI;
+  uint8_t magI;
+  int8_t ofsQ;
+  uint8_t magQ;
+  uint8_t cfgSource;
+  uint8_t reserved1[3];
+  uint32_t lowLevCfg;
+  uint8_t reserved2[8];
+  uint32_t postStatus;
+  uint8_t reserved3[4];
+} mon_hw2_msg;
+
 namespace ublox {
   // protocol constants
   const uint8_t PREAMBLE1 = 0xb5;
@@ -125,6 +138,7 @@ namespace ublox {
 
   // MON messages
   const uint8_t MSG_MON_HW = 0x09;
+  const uint8_t MSG_MON_HW2 = 0x0B;
 
   const int UBLOX_HEADER_SIZE = 6;
   const int UBLOX_CHECKSUM_SIZE = 2;
@@ -139,6 +153,7 @@ namespace ublox {
       kj::Array<capnp::word> gen_solution();
       kj::Array<capnp::word> gen_raw();
       kj::Array<capnp::word> gen_mon_hw();
+      kj::Array<capnp::word> gen_mon_hw2();
 
       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.cc

@@ -1,29 +1,11 @@
 #include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <signal.h>
-#include <unistd.h>
-#include <sched.h>
-#include <sys/time.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <assert.h>
-#include <math.h>
-#include <ctime>
-#include <chrono>
 
 #include "messaging.hpp"
 
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/timing.h"
-
 #include "ublox_msg.h"
 
-const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
-
 Message * poll_ubloxraw_msg(Poller * poller) {
-  auto p = poller->poll(ZMQ_POLL_TIMEOUT);
+  auto p = poller->poll(1000);
 
   if (p.size()) {
     return p[0]->receive();
@@ -32,11 +14,10 @@ Message * poll_ubloxraw_msg(Poller * poller) {
   }
 }
 
-
 int send_gps_event(PubSocket *s, const void *buf, size_t len) {
   return s->send((char*)buf, len);
 }
 
 int main() {
   return ubloxd_main(poll_ubloxraw_msg, send_gps_event);
-}
+}

selfdrive/locationd/ubloxd_main.cc

@@ -1,9 +1,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <signal.h>
 #include <unistd.h>
-#include <sched.h>
 #include <errno.h>
 #include <sys/time.h>
 #include <sys/cdefs.h>
@@ -21,17 +19,10 @@
 
 #include "ublox_msg.h"
 
-volatile sig_atomic_t do_exit = 0; // Flag for process exit on signal
-
-void set_do_exit(int sig) {
-  do_exit = 1;
-}
-
+ExitHandler do_exit;
 using namespace ublox;
 int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
   LOGW("starting ubloxd");
-  signal(SIGINT, (sighandler_t) set_do_exit);
-  signal(SIGTERM, (sighandler_t) set_do_exit);
 
   UbloxMsgParser parser;
 
@@ -100,6 +91,13 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
               auto bytes = words.asBytes();
               pm.send("ubloxGnss", bytes.begin(), bytes.size());
             }
+          } else if(parser.msg_id() == MSG_MON_HW2) {
+            //LOGD("MSG_MON_HW2");
+            auto words = parser.gen_mon_hw2();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              pm.send("ubloxGnss", bytes.begin(), bytes.size());
+            }
           } else {
             LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
           }

selfdrive/locationd/ubloxd_test.cc

@@ -1,93 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <signal.h>
-#include <unistd.h>
-#include <sched.h>
-#include <sys/time.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <assert.h>
-#include <math.h>
-#include <ctime>
-#include <chrono>
-#include <iostream>
-
-#include "messaging.hpp"
-#include "impl_zmq.hpp"
-
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/timing.h"
-#include "common/util.h"
-#include "ublox_msg.h"
-
-using namespace ublox;
-extern volatile sig_atomic_t do_exit;
-
-void write_file(std::string fpath, uint8_t *to_write, int length) {
-  FILE* f = fopen(fpath.c_str(), "wb");
-  if (!f) {
-    std::cout << "Open " << fpath << " failed" << std::endl;
-    return;
-  }
-  fwrite(to_write, length, 1, f);
-  fclose(f);
-}
-
-static size_t len = 0U;
-static size_t consumed = 0U;
-static uint8_t *data = NULL;
-static int save_idx = 0;
-static std::string prefix;
-
-Message * poll_ubloxraw_msg(Poller * poller) {
-  assert(poller);
-
-  size_t consuming  = min(len - consumed, 128);
-  if(consumed < len) {
-    // create message
-    MessageBuilder msg_builder;
-    auto ublox_raw = msg_builder.initEvent().initUbloxRaw(consuming);
-    memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
-
-    auto bytes = msg_builder.toBytes();
-
-    Message * msg = new ZMQMessage();
-    msg->init((char*)bytes.begin(), bytes.size());
-    consumed += consuming;
-    return msg;
-  } else {
-    do_exit = 1;
-    return NULL;
-  }
-}
-
-int send_gps_event(PubSocket *s, const void *buf, size_t length) {
-  assert(s);
-  write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, length);
-  save_idx++;
-  return length;
-}
-
-int main(int argc, char** argv) {
-  if(argc < 3) {
-    printf("Format: ubloxd_test stream_file_path save_prefix\n");
-    return 0;
-  }
-  // Parse 11360 msgs, generate 9452 events
-  data = (uint8_t *)read_file(argv[1], &len);
-  if(data == NULL) {
-    LOGE("Read file %s failed\n", argv[1]);
-    return -1;
-  }
-  prefix = argv[2];
-  ubloxd_main(poll_ubloxraw_msg, send_gps_event);
-  free(data);
-  printf("Generated %d cereal events\n", save_idx);
-  if(save_idx != 9452) {
-    printf("Event count error: %d\n", save_idx);
-    return -1;
-  }
-  return 0;
-}