Hi Modal,

When upping the IMU_GYRO_RATEMAX (for lower end-to-end worse case latency in the gyro movement -> actuation loop), I'm seeing the DSP load jump (measured by px4-listener cpuload). After digging into it, it looks like the main culprit is not the processing modules, but rather the FIFO read (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) in the ICM42688P driver) and the UART write (uart_write(&data, sizeof(DataPacket)) != sizeof(DataPacket)) to our STM board (for actuation).

About 40% of the DSP is being used for the FIFO transfer and the UART write with IMU_GYRO_RATEMAX set to 1kHz (1000Hz).

Any recommendations on how to minimize the DSP being choked by these IO operations?

I can only think of marginal gains like decreasing how many bytes the FIFO is configured to contain, or optimizing our DataPacket size down to like 12 bytes (currently 17 bytes). But maybe there is some more fundamental strategy that I'm missing? Or is IO just generally a bad time for the DSP?

Thanks for any advice!

Rowan (Cleo Robotics)

Hey guys any words of advice here?

Hi ModalAI,

We at Cleo have been corresponding with PMD (the ToF supplier) about how to tune the ToF for optimal tradeoff between false positives (floating artifacts) and false negatives (not seeing dark surfaces). One of the main action items from our discussion (which I had ChatGPT summarize below for your reference) is the need to record what PMD calls the "RoyaleRecordingFile" format from the ToF. This will unlock the ability to tune processing parameters on a dataset of pre-recorded files, which makes large scale tuning feasible. Has ModalAI looked into producing the "RoyaleRecordingFile" format from the ToF using the VOXL2?

Thank you,

Rowan

Summary of Email Correspondence on Recording RRF Files on VOXL2

The core discussion revolves around whether it is feasible to record RRF (Royale Recording Format) files on the VOXL2 platform, which is used by Cleo Robotics in conjunction with a 3D Time-of-Flight (ToF) sensor.

1. Motivation: Reducing False Positives and False Negatives

Cleo Robotics has observed conflicting effects from tuning software parameters:

• Increasing exposure time and adjusting detection thresholds sometimes improves detection of dark surfaces but introduces false positives (e.g., floating points or noise on edges).
• Conversely, stricter thresholds reduce false positives but lead to false negatives, especially with low-reflectivity (dark) objects.

This trade-off has motivated the team to find an optimal set of processing parameters that minimizes both FP and FN. To do this effectively, they are seeking a way to record RRF files, which would enable offline experimentation and replay with different tuning values.

2. VOXL2 Limitations and Inquiry to PMD

Rowan Dempster reached out to PMD Technologies to:

• Ask whether RRF files can be recorded directly from the 3D ToF sensor on VOXL2.
• Understand the value of producing RRF files and whether recorded data could be “played back” with alternative parameters for comparative analysis.
• Explore potential development board solutions for this purpose.

3. PMD’s Feedback and ModalAI Software Potential

Tim Werner from PMD explained that:

• The VOXL2 platform does not natively support RRF recording.
• However, the underlying software stack used by ModalAI includes this capability through CAMX, suggesting it is technically feasible.
• Rowan was encouraged to contact ModalAI to request access or support for enabling this feature.
• PMD expressed willingness to assist ModalAI if needed.

4. Cleo’s Proposed Workaround and Implementation Details

Rowan noted:

• They are considering using a separate development board (if available) that supports RRF recording for tuning.
• Once optimal parameters are found, the same could be transferred to the VOXL2 by modifying the 3D ToF driver (e.g., setSpectreParams in VOXL SDK).
• This avoids the repeated burden of recompiling and redeploying firmware for each test case.

Rowan also provided a Google Drive link and GitLab reference to their current tuning approach for PMD’s review.

5. PMD’s Supplemental Suggestions

In addition to the ModalAI pathway, PMD suggested:

• Cleo could share their current parameter values.
• PMD might then provide tuning suggestions to improve detection performance, although this would be less precise than replaying real RRF data.

Conclusion

• Recording RRF files on VOXL2 is not currently supported, but may be possible with cooperation from ModalAI.
• Cleo’s goal is to optimize ToF sensor parameters to reduce both false positives and false negatives, particularly for dark surfaces.
• Next steps involve contacting ModalAI to explore unlocking RRF functionality, with potential support from PMD.
• In parallel, Cleo can continue parameter tuning directly on VOXL2 through driver-level code adjustments, albeit with more effort.

If helpful, we’d be happy to provide a concise technical summary of our current parameter usage or collaborate on getting the RRF feature exposed.

@Alex-Kushleyev Gotcha, thanks for the info Alex and your help resolving this issue!

@Alex-Kushleyev

So, please try the code in the branch (with frame id fix) with different roi sources and let me know how it behaves. It should not matter whether the camera is IMX214 or IMX412, but i have not tried with IMX214. If this branch has the issue with IMX214 based on your testing, i can try it.

Is there any reason to use the preview source? I would prefer to leave that stream disabled since we do not need it for anything except (in the past) the ROI pipe (but now that ROI works on the large video source), and we are already using the large video pipe for recording videos.

Also, if you have not already done so on your branch, please lock voxl-camera-server to use only upper 4 cores, as it significantly improves camera server performance when the load is high (the low level ISP interrupts happen on lower cores only, so this way camera server is not interfering). The reason why i mention this is that you are using an old branch of camera server, so just double checking.

Yes we have the voxl-camera-server locked to 4-7:

[Unit]
Description=voxl-camera-server
SourcePath=/usr/bin/voxl-camera-server
After=voxl-wait-for-fs.service
Requires=voxl-wait-for-fs.service

[Service]
User=root
Type=simple
PIDFile=/run/voxl-camera-server.pid
ExecStart=/usr/bin/taskset -c 4-7 /usr/bin/voxl-camera-server

@Alex-Kushleyev Looking at that branch you sent, I might see why you are having better luck than us. We at Cleo only have the code up to and including add roi transition filter to control command; re-enable publishing of preview...

In green are the commits we have in our codebase, in red are the commits we do not have.

I note that in the commits we do not have you added the option to change the ROI pipe source:

    enum ROI_SOURCE_STREAM {
        ROI_SOURCE_PREVIEW,
        ROI_SOURCE_VIDEO_SMALL,
        ROI_SOURCE_VIDEO_LARGE
    };

and default it to

        roi_source_stream    = ROI_SOURCE_VIDEO_LARGE; //ROI_SOURCE_PREVIEW; //ROI_SOURCE_VIDEO_SMALL // ROI_SOURCE_VIDEO_LARGE

Perhaps in the code you tried yesterday the ROI source is the large encoded stream, whereas with my version of the code the ROI source is the preview stream. I will try to port over those five most recent commits from your work on ROI that we do not have in our code base and hopefully get the ROI pipe to work well using the large encoded stream, and that will serve as a solution (or rather no longer have the need) to get the preview stream working alongside the large encoded stream. Thank you for your help!!

@Alex-Kushleyev Hi Alex,

I am assuming that you are using non-raw preview stream (YUV) at resolution around 4000x3000 and similar resolution for the large encoded stream. Then the ROI stream is the downscaled version of the preview stream (please correct me if I am wrong).

That is correct yes.

I just tested this setup using IMX412 camera, and using the following branch (which I believe you used as a starting point):
what camera you are using

We are using the IMX214, not the IMX 412 - not sure if that would cause a different outcome

Also try disabling all other cameras and seeing if the issue goes away.

Only running the hires camera -> the issue persists.

voxl-camera-server.conf config section for that camera (assuming other cameras do not interfere)

Other cameras do not seem to impact results. Here is the config entry

	"cameras":	[{
			"type":	"imx214",
			"name":	"hires",
			"enabled":	true,
			"camera_id":	0,
			"fps":	25,
			"en_preview":	true,
			"preview_width":	4208,
			"preview_height":	3120,
			"en_raw_preview":	false,
			"en_small_video":	true,
			"small_video_width":	1024,
			"small_video_height":	768,
			"small_venc_mode":	"h265",
			"small_venc_br_ctrl":	"cqp",
			"small_venc_Qfixed":	30,
			"small_venc_Qmin":	15,
			"small_venc_Qmax":	40,
			"small_venc_nPframes":	9,
			"small_venc_mbps":	2,
			"en_large_video":	true,
			"large_video_width":	4208,
			"large_video_height":	3120,
			"large_venc_mode":	"h265",
			"large_venc_br_ctrl":	"cqp",
			"large_venc_Qfixed":	42,
			"large_venc_Qmin":	17,
			"large_venc_Qmax":	52,
			"large_venc_nPframes":	29,
			"large_venc_mbps":	30,
			"en_snapshot":	false,
			"en_snapshot_width":	1024,
			"en_snapshot_height":	768,
			"en_roi":	true,
			"roi_width":	1152,
			"roi_height":	720,
			"ae_mode":	"isp"
		}]

https://gitlab.com/voxl-public/voxl-sdk/services/voxl-camera-server/-/tree/feature-add-c2d-select-roi-src/

I will try out this code now!

During a recent flight the voxl-imu-server stopped producing data with this error:

May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54514 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54515 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54516 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54517 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54518 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54519 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54520 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]: ERROR icm42688_fifo_read: buffer was empty 54521 times in a row
May 09 19:49:48 m0054 voxl-imu-server[1750]:  imu 0
May 09 19:49:48 m0054 voxl-imu-server[1750]: WARNING bad reading on imu 0

after restarting the service it failed again

May 09 19:51:14 m0054 voxl-imu-server[7425]: aux_imu3_enable:                 0
May 09 19:51:14 m0054 voxl-imu-server[7425]: aux_imu3_spi_bus:                5
May 09 19:51:14 m0054 voxl-imu-server[7425]: aux_imu3_sample_rate_hz:         1000.0
May 09 19:51:14 m0054 voxl-imu-server[7425]: aux_imu3_lp_cutoff_freq_hz:      92.0
May 09 19:51:14 m0054 voxl-imu-server[7425]: aux_imu3_fifo_poll_rate_hz:      100.0
May 09 19:51:14 m0054 voxl-imu-server[7425]: =================================================================
May 09 19:51:14 m0054 voxl-imu-server[7425]: thread is locked to cores: 0 1 2 3
May 09 19:51:14 m0054 voxl-imu-server[7425]: Detected M0054 VOXL2
May 09 19:51:14 m0054 voxl-imu-server[7425]: Initializing IMU0
May 09 19:51:14 m0054 systemd[1]: voxl-imu-server.service: Failed with result 'exit-code'.

after rebooting the voxl2 it was fine again, but I'm sure it will happen again given enough time, already seen it twice.

Any ideas what is up here or just a faulty voxl2? We have dozens of other voxl2s that haven't seen this issue.

@Alex-Kushleyev Also if it's helpful here's all the source code we are using for the camera server https://drive.google.com/file/d/1ZjcIAvNpQpGPDnzcirZ4OE0acJhEDwZH/view?usp=sharing

@Alex-Kushleyev Hi Alex, based on the change log we have up to 1.9.2:
Here's a snippet

1.9.2
    * fix -C22 for D0013
1.9.1
    * liow2 tof
    * new royale support
1.9.0
    * targeted for SDK 1.2.x
    * swap to tof2_data_t type for tof publication to MPA
    * support for new voxl-fsync kernel module for synced cameras
    * temperarily add ar0144_slave drivers
    * new build dependency on voxl-cross:V2.7
    * (qrb5165) requires new system image with royale-5-8-spectre-6-10
1.8.10
    * fix cpu monitor not starting libmodalpipe helper (tof decimator feature)
1.8.9
    * allow duplicate cam id's in config file as long as they are disabled
    * parse independent exposure setting if stereo cam has secondary id=0
    * start the gps client later, after all normal setup steps
1.8.8
    * add feature to attempt skipping potentially unplugged cameras during startup
    * fix bug that might retrieve gain limits incorrectly where camId doesn't match the camera index in config file (rare)
    * now exit with an error if no cameras could be started instead of spinning forever.
    * add temporary fixed ar0144 driver
    * change cam config 20 for D0011 Starling PX4 edition
    * add cci-direct tweaks to set ov7251 blc to every frame. fixes flicker
1.8.7
    * fix bug where color images debayered to RGB still said NV12 in the pipe info json
    * add support for rotating AR0144 image
1.8.6
    * put old exposure settings back for now until we do testing
    * more ar0144 support
    * add exposure and gain upper and lower limits to config file
1.8.5
    * add cam config 22 for D0013
    * fix sentinel front stereo flip on old system image
    * fix jpeg snapshots being cut short by 1 byte
1.8.4
    * voxl-configure-cameras better handling of new custom config file when fresh/missing
    * dynamically set libmodal_exposure gain limits based on hal3 reported numbers in prep for system image 1.7.x gain scaling change
    * set C18 config name
    * favor faster exposure times for vio cameras
1.8.3
    * config file now has simple boolean for raw data preview true/false
    * remove old pre_format field from config file
    * fix regression in debayer processing
    * white balance for m0113 ov9782 without IR filter
    * add single mode option for combo sensors
    * allow both "snapshot-no-save" and "snapshot_no_save" commands```