diff --git a/README.md b/README.md index 2f09d06..f4488e5 100644 --- a/README.md +++ b/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 diff --git a/include/metavision_driver/metavision_wrapper.h b/include/metavision_driver/metavision_wrapper.h index 35bfd50..4b7b3bb 100644 --- a/include/metavision_driver/metavision_wrapper.h +++ b/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 roi_; std::string sensorVersion_{"0.0"}; diff --git a/src/driver_ros1.cpp b/src/driver_ros1.cpp index 7030c37..bc68fce 100644 --- a/src/driver_ros1.cpp +++ b/src/driver_ros1.cpp @@ -231,6 +231,8 @@ void DriverROS1::configureWrapper(const std::string & name) nh_.param("erc_mode", "na"), // Event Rate Controller Mode nh_.param("erc_rate", 100000000)); // Event Rate Controller Rate + wrapper_->setMIPIFramePeriod(nh_.param("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())); diff --git a/src/driver_ros2.cpp b/src/driver_ros2.cpp index 7b50580..d96b36f 100644 --- a/src/driver_ros2.cpp +++ b/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) diff --git a/src/metavision_wrapper.cpp b/src/metavision_wrapper.cpp index 6e5efd9..020f24c 100644 --- a/src/metavision_wrapper.cpp +++ b/src/metavision_wrapper.cpp @@ -24,6 +24,7 @@ #endif #include +#include #include #include @@ -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(); + 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_) {