openpilot v0.6.3 release

old-commit-hash: d5f9caa82d

selfdrive/locationd/calibrationd.py

@@ -8,7 +8,7 @@ from selfdrive.locationd.calibration_helpers import Calibration
 from selfdrive.swaglog import cloudlog
 from selfdrive.services import service_list
 from common.params import Params
-from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_medmodel_frame
+from common.transformations.model import model_height
 from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
                                           eon_intrinsics, get_calib_from_vp, H, W
 
@@ -78,21 +78,19 @@ class Calibrator(object):
                       "valid_points": len(self.vps)}
         self.params.put("CalibrationParams", json.dumps(cal_params))
       return new_vp
+    else:
+      return None
 
   def send_data(self, livecalibration):
     calib = get_calib_from_vp(self.vp)
     extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
-    ke = eon_intrinsics.dot(extrinsic_matrix)
-    warp_matrix = get_camera_frame_from_model_frame(ke)
-    warp_matrix_big = get_camera_frame_from_medmodel_frame(ke)
 
     cal_send = messaging.new_message()
     cal_send.init('liveCalibration')
     cal_send.liveCalibration.calStatus = self.cal_status
     cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
-    cal_send.liveCalibration.warpMatrix2 = [float(x) for x in warp_matrix.flatten()]
-    cal_send.liveCalibration.warpMatrixBig = [float(x) for x in warp_matrix_big.flatten()]
     cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
+    cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
 
     livecalibration.send(cal_send.to_bytes())
 

selfdrive/locationd/locationd_yawrate.cc

@@ -12,9 +12,11 @@
 
 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;
-  prev_update_time = current_time;
-  if (dt < 1.0e-9) {
-    return;
+
+  if (dt < 0) {
+    dt = 0;
+  } else {
+    prev_update_time = current_time;
   }
 
   // x = A * x;
@@ -73,6 +75,7 @@ Localizer::Localizer() {
 void Localizer::handle_log(cereal::Event::Reader event) {
   double current_time = event.getLogMonoTime() / 1.0e9;
 
+  // Initialize update_time on first update
   if (prev_update_time < 0) {
     prev_update_time = current_time;
   }
@@ -117,6 +120,16 @@ extern "C" {
     Localizer * loc = (Localizer*) localizer;
     return loc->x[1];
   }
+
+  void localizer_set_yaw(void * localizer, double yaw) {
+    Localizer * loc = (Localizer*) localizer;
+    loc->x[0] = yaw;
+  }
+  void localizer_set_bias(void * localizer, double bias) {
+    Localizer * loc = (Localizer*) localizer;
+    loc->x[1] = bias;
+  }
+
   double localizer_get_t(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
     return loc->prev_update_time;

selfdrive/locationd/paramsd.cc

@@ -31,9 +31,9 @@ int main(int argc, char *argv[]) {
   zmq_pollitem_t polls[num_polls] = {{0}};
   polls[0].socket = controls_state_sock;
   polls[0].events = ZMQ_POLLIN;
-  polls[1].socket = sensor_events_sock;
+  polls[1].socket = camera_odometry_sock;
   polls[1].events = ZMQ_POLLIN;
-  polls[2].socket = camera_odometry_sock;
+  polls[2].socket = sensor_events_sock;
   polls[2].events = ZMQ_POLLIN;
 
   // Read car params
@@ -122,8 +122,9 @@ int main(int argc, char *argv[]) {
         localizer.handle_log(event);
 
         auto which = event.which();
+        // Throw vision failure if posenet and odometric speed too different
         if (which == cereal::Event::CAMERA_ODOMETRY){
-          if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.5 * localizer.car_speed, 5.0)) {
+          if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
               posenet_invalid_count++;
             } else {
             posenet_invalid_count = 0;
@@ -155,7 +156,7 @@ int main(int argc, char *argv[]) {
             live_params.setStiffnessFactor(learner.x);
             live_params.setSteerRatio(learner.sR);
             live_params.setPosenetSpeed(localizer.posenet_speed);
-            live_params.setPosenetValid(posenet_invalid_count < 5);
+            live_params.setPosenetValid(posenet_invalid_count < 4);
 
             auto words = capnp::messageToFlatArray(msg);
             auto bytes = words.asBytes();