ability to set mipi_frame_period

README.md

@@ -120,6 +120,8 @@ Parameters:
   specifying serial number and look at the log files.
 - ``event_message_time_threshold``: (in seconds) minimum time span of
   events to be aggregated in one ROS event message before message is sent. Defaults to 1ms.
+  In its default setting however the SDK provides packets only every 4ms. To increase SDK
+  callback frequency, tune ``mipi_frame_period`` if available for your sensor.
 - ``event_message_size_threshold``: (in bytes) minimum size of events
   (in bytes) to be aggregated in one ROS event message before message is sent. Defaults to 1MB.
 - ``statistics_print_interval``: time in seconds between statistics printouts.
@@ -142,6 +144,8 @@ Parameters:
 - ``erc_mode``: event rate control mode (Gen4 sensor): ``na``,
   ``disabled``, ``enabled``. Default: ``na``.
 - ``erc_rate``: event rate control rate (Gen4 sensor) events/sec. Default: 100000000.
+- ``mipi_frame_period``:: mipi frame period in usec. Only available on some sensors.
+    Tune this to get faster callback rates from the SDK to the ROS driver. For instance 1008 will give a callback every millisecond. Risk of data corruption when set too low! Default: -1 (not set).
 - ``sync_mode``: Used to synchronize the time stamps across multiple
   cameras (tested for only 2). The cameras must be connected via a
   sync cable, and two separate ROS driver nodes are started, see

include/metavision_driver/metavision_wrapper.h

@@ -109,6 +109,7 @@ class MetavisionWrapper
     ercMode_ = mode;
     ercRate_ = rate;
   }
+  void setMIPIFramePeriod(int usec) { mipiFramePeriod_ = usec; }
 
   bool triggerActive() const
   {
@@ -136,6 +137,7 @@ class MetavisionWrapper
     const std::string & mode_in, const std::string & mode_out, const int period,
     const double duty_cycle);
   void configureEventRateController(const std::string & mode, const int rate);
+  void configureMIPIFramePeriod(int usec, const std::string & sensorName);
   void printStatistics();
   // ------------ variables
   CallbackHandler * callbackHandler_{0};
@@ -164,6 +166,7 @@ class MetavisionWrapper
   HardwarePinConfig hardwarePinConfig_;
   std::string ercMode_;
   int ercRate_;
+  int mipiFramePeriod_{-1};
   std::string loggerName_{"driver"};
   std::vector<int> roi_;
   std::string sensorVersion_{"0.0"};

src/driver_ros1.cpp

@@ -231,6 +231,8 @@ void DriverROS1::configureWrapper(const std::string & name)
     nh_.param<std::string>("erc_mode", "na"),  // Event Rate Controller Mode
     nh_.param<int>("erc_rate", 100000000));    // Event Rate Controller Rate
 
+  wrapper_->setMIPIFramePeriod(nh_.param<int>("mipi_frame_period", -1));
+
   // Get information on external pin configuration per hardware setup
   if (wrapper_->triggerActive()) {
     wrapper_->setHardwarePinConfig(get_hardware_pin_config(nh_, ros::this_node::getName()));

src/driver_ros2.cpp

@@ -347,6 +347,9 @@ void DriverROS2::configureWrapper(const std::string & name)
   if (wrapper_->triggerActive()) {
     wrapper_->setHardwarePinConfig(get_hardware_pin_config(this));
   }
+  int mipiFramePeriod{-1};
+  this->get_parameter_or("mipi_frame_period", mipiFramePeriod, -1);
+  wrapper_->setMIPIFramePeriod(mipiFramePeriod);
 }
 
 void DriverROS2::rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end)

src/metavision_wrapper.cpp

@@ -24,6 +24,7 @@
 #endif
 
 #include <metavision/hal/facilities/i_hw_identification.h>
+#include <metavision/hal/facilities/i_hw_register.h>
 #include <metavision/hal/facilities/i_plugin_software_info.h>
 #include <metavision/hal/facilities/i_trigger_in.h>
 
@@ -321,6 +322,7 @@ bool MetavisionWrapper::initializeCamera()
     sensorVersion_ =
       std::to_string(sinfo.major_version_) + "." + std::to_string(sinfo.minor_version_);
     LOG_INFO_NAMED("sensor version: " << sensorVersion_);
+    LOG_INFO_NAMED("sensor name: " << sinfo.name_);
     if (!biasFile_.empty()) {
       try {
         cam_.biases().set_from_file(biasFile_);
@@ -351,6 +353,9 @@ bool MetavisionWrapper::initializeCamera()
       configureExternalTriggers(
         triggerInMode_, triggerOutMode_, triggerOutPeriod_, triggerOutDutyCycle_);
       configureEventRateController(ercMode_, ercRate_);
+      if (mipiFramePeriod_ > 0) {
+        configureMIPIFramePeriod(mipiFramePeriod_, sinfo.name_);
+      }
     }
     statusChangeCallbackId_ = cam_.add_status_change_callback(
       std::bind(&MetavisionWrapper::statusChangeCallback, this, ph::_1));
@@ -370,6 +375,20 @@ bool MetavisionWrapper::initializeCamera()
   return (true);
 }
 
+void MetavisionWrapper::configureMIPIFramePeriod(int usec, const std::string & sensorName)
+{
+  if (sensorName == "IMX636") {
+    const uint32_t mfpa = 0xB028;  // for imx636
+    auto hwrf = cam_.get_device().get_facility<Metavision::I_HW_Register>();
+    const int prev_mfp = hwrf->read_register(mfpa);
+    hwrf->write_register(mfpa, usec);
+    const int new_mfp = hwrf->read_register(mfpa);
+    LOG_INFO_NAMED("mipi frame period changed from " << prev_mfp << " to " << new_mfp << "us");
+  } else {
+    LOG_WARN_NAMED("cannot configure mipi frame period for sensor " << sensorName);
+  }
+}
+
 void MetavisionWrapper::setDecodingEvents(bool decodeEvents)
 {
   if (decodeEvents && !contrastCallbackActive_) {