Skip to content
Permalink
Browse files

Added dynamic_reconfigure for frame rate configuration

  • Loading branch information
luca-della-vedova committed Nov 7, 2019
1 parent 5ab886e commit 4a00ea16d390fc476e8c798bc9f8ca67acf5a55b
@@ -7,5 +7,6 @@ gen = ParameterGenerator()

gen.add("fast_enable", bool_t, 0, "Enable FAST corner detection", True)
gen.add("fast_threshold", int_t, 0, "Threshold for FAST corner detection", 50, 0, 255)
gen.add("frame_downsampling", int_t, 0, "Downsampling rate between IMU and imagers", 7, 7, 255)

exit(gen.generate(PACKAGE, "ovc_embedded_driver", "Ovc3"))
@@ -55,6 +55,7 @@ class ICMDriver : public SPIDriver

// AXI GPIO addresses
static constexpr unsigned int GPIO_DATA = 0x0000 / sizeof(unsigned int);
static constexpr unsigned int GPIO2_DATA = 0x0008 / sizeof(unsigned int);
static constexpr unsigned int GIER = 0x011C / sizeof(unsigned int);
static constexpr unsigned int IER = 0x0128 / sizeof(unsigned int);
static constexpr unsigned int ISR = 0x0120 / sizeof(unsigned int);
@@ -73,6 +74,7 @@ class ICMDriver : public SPIDriver
ICMDriver(int spi_num, int gpio_uio_num);
int getSampleNumber();
IMUReading readSensors();
void setFrameDownsample(uint8_t downsample);
};

class VNAVDriver : public SPIDriver
@@ -138,8 +138,9 @@ void configureFAST(bool enable, int thresh)

void reconfigure_callback(Ovc3Config &conf, uint32_t level)
{
// TODO add more reconfigurability
configureFAST(conf.fast_enable, conf.fast_threshold);
// Configure frame downsampling (frame rate will be IMU rate / frame_downsampling)
spi.setFrameDownsample(conf.frame_downsampling);
}

int main(int argc, char **argv)
@@ -156,10 +157,11 @@ int main(int argc, char **argv)
server.setCallback(c);
ros::AsyncSpinner spinner(1);

std::unique_ptr<ImagePublisher> image_publishers[NUM_CAMERAS];
std::unique_ptr<ExternalCameraPublisher> external_publishers[EXTERNAL_BOARDS];

// Init threads
std::vector<std::unique_ptr<std::thread>> threads; // one each for IMU and frame_time_ptr update threads
std::unique_ptr<ImagePublisher> image_publisher[NUM_CAMERAS];
std::unique_ptr<ExternalCameraPublisher> external_publishers[EXTERNAL_BOARDS];

// IMU thread
threads.push_back(std::make_unique<std::thread>(publish_imu, nh, frame_time_ptr, curr_time_ptr));
@@ -173,8 +175,8 @@ int main(int argc, char **argv)
for (size_t i=0; i<NUM_CAMERAS; ++i)
{
CameraHWParameters params(DMA_DEVICES[i], I2C_DEVICES[i], 0, CAMERA_NAMES[i], i == COLOR_CAMERA_ID);
image_publisher[i] = std::make_unique<ImagePublisher>(nh, params, frame_time_ptr);
threads.push_back(std::make_unique<std::thread>(&ImagePublisher::publish_loop, image_publisher[i].get()));
image_publishers[i] = std::make_unique<ImagePublisher>(nh, params, frame_time_ptr);
threads.push_back(std::make_unique<std::thread>(&ImagePublisher::publish_loop, image_publishers[i].get()));
}
// External camera boards
// For now only add one external pair
@@ -76,14 +76,18 @@ ICMDriver::ICMDriver(int spi_num, int gpio_uio_num) :
// Enable interrupts on channel 0
uio.writeRegister(GIER, 1 << 31);
uio.writeRegister(IER, 1);
//uio.writeRegister(8, 7); // 7 samples per frame
// We need to write 3 to ISR register to toggle both (should only be 1?)
uio.setResetRegisterMask(ISR, 3);
// Configure interrupt on sample ready
writeRegister(INT_ENABLE_1, 1);
std::cout << "ICM IMU Initialization done" << std::endl;
}

void ICMDriver::setFrameDownsample(uint8_t downsample)
{
uio.writeRegister(GPIO2_DATA, downsample); // 7 samples per frame
}

void ICMDriver::selectBank(int bank)
{
writeRegister(REG_BANK_SEL, bank << 4);

0 comments on commit 4a00ea1

Please sign in to comment.
You can’t perform that action at this time.