Time Of Flight (TOF) camera output FPS divided by 5 after upgrading from SDK 1.5.0 to SDK 1.6.3 (Starling2 Max C29)

qt

I would like to report a probable bug related to the TOF camera on my Starling 2 Max (configuration C29).
I was previously running SDK 1.5.0, and the TOF camera was operating correctly at the frame rate specified in the file "/etc/modalai/voxl-camera-server.conf".
I recently upgraded from SDK 1.5.0 to SDK 1.6.3 (I didn't try other SDK versions).
Since then, the TOF camera runs at a frequency divided by 5. I tested this using the CLI inspection tool with the command "voxl-inspect-cam tof_depth" for different FPS values (same results in VOXL-Portal). I have not changed anything on the drone except the "fps" parameter in the configuration file "/etc/modalai/voxl-camera-server.conf".
Here is my TOF camera configuration:

"type": "pmd-tof-liow2",
"name": "tof",
"enabled":      true,
"camera_id":    3,
"fps":  10,
"en_rotate":    true,
"ae_mode":      "off",
"gain_min":     0,
"gain_max":     0,
"exposure_max_us":      6000,
"standby_enabled":      false,
"decimator":    5

Here are the results from "voxl-inspect-cam tof_depth" for different FPS values:

fps = 5
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 | 48.00 |    0 |       26 |     28.1  |  1.0 |    0.3 | RAW8

fps = 10
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 | 24.00 |    0 |       28 |     27.2  |  2.0 |    0.7 | RAW8

fps = 15
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 | 16.00 |    0 |       16 |     28.6  |  3.0 |    1.0 | RAW8

fps = 20
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 | 12.00 |    0 |       21 |     27.8  |  4.0 |    1.4 | RAW8

fpx = 30
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  8.00 |    0 |       13 |     26.8  |  6.0 |    2.1 | RAW8

fps = 60
| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  4.00 |    0 |      288 |     10.0  | 12.0 |    4.1 | RAW8


fps = (8, 12, 25, 35, 40, 45, 50, 65, 70, 75, 80, 85, 90, 100, 120, 135, 140, 150, 180)

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth | Server Disconnected

Looking at the code in "src/hal3_camera_mgr.cpp", it seems expected that other frame rates do not work anymore. Here is the relevant section:

if (configInfo.type == SENSOR_TOF){
    if (configInfo.fps != 5 && configInfo.fps != 10 && configInfo.fps != 15 && configInfo.fps != 30 && configInfo.fps != 45){
        M_ERROR("Invalid TOF framerate: %d, must be either 5, 10, 15, 30, or 45\n", configInfo.fps);
        return -1;
    }
}

if (configInfo.type == SENSOR_TOF_LIOW2){
    if (configInfo.fps != 5 && configInfo.fps != 10 && configInfo.fps != 15 && configInfo.fps != 20 && configInfo.fps != 30 && configInfo.fps != 60){
        M_ERROR("Invalid TOF framerate: %d, must be either 5, 10, 15, 20, 30, or 60\n", configInfo.fps);
        return -1;
    }
}

Below are the full version details for SDK 1.6.3 (where the issue occurs):

voxl2:~$ voxl-version
────────────────────────────────────────────────────────────────────────────────
system-image: 1.8.06-M0054-14.1a-perf
kernel:       #1 SMP PREEMPT Wed Oct 22 04:13:18 UTC 2025 4.19.125
────────────────────────────────────────────────────────────────────────────────
hw platform:  M0054
mach.var:     1.0.1
SKU:          MRB-D0012-4-V2-C29-T9-M28-X0
────────────────────────────────────────────────────────────────────────────────
voxl-suite:   1.6.3
────────────────────────────────────────────────────────────────────────────────
Packages:
Repo:  http://voxl-packages.modalai.com/ ./dists/qrb5165/sdk-1.6/binary-arm64/
Last Updated: 2026-03-09 17:52:53
List:
libfc-sensor                   1.0.9
libmodal-cv                    0.6.0
libmodal-exposure              0.1.4
libmodal-flow                  1.0.3
libmodal-journal               0.2.7
libmodal-json                  0.4.8
libmodal-pipe                  2.14.11
libqrb5165-io                  0.6.3
libvoxl-cci-direct             0.3.3
libvoxl-codec                  0.0.2
libvoxl-cutils                 0.1.6
modalai-slpi                   1.2.2
mv-voxl                        0.1-r0
qrb5165-bind                   0.1-r0
qrb5165-dfs-server             0.2.0
qrb5165-mini-tof-server        0.2.2
qrb5165-rangefinder-server     0.1.6
qrb5165-slpi-test-sig          01-r0
qrb5165-tflite                 2.17.2
voxl-bind-spektrum             0.1.1
voxl-camera-calibration        0.6.1
voxl-camera-server             2.2.19
voxl-cassie-ros2               0.0.1
voxl-ceres-solver              2:2.0.0-2
voxl-configurator              1.1.5
voxl-cpu-monitor               0.7.7
voxl-docker-support            1.3.1
voxl-elrs                      1.1.0
voxl-esc                       1.5.7
voxl-esptool                   0.2.0
voxl-feature-tracker           0.5.2
voxl-flow-server               0.3.6
voxl-gphoto2-server            0.0.10
voxl-imu-server                2.0.1
voxl-io-server                 0.0.8
voxl-jpeg-turbo                2.1.3-7
voxl-lepton-server             1.3.3
voxl-lepton-tracker            0.0.4
voxl-libgeographic             1.0.0
voxl-libgphoto2                0.0.4
voxl-libuvc                    1.0.7
voxl-logger                    0.6.1
voxl-mavcam-manager            0.6.0
voxl-mavlink                   0.1.6
voxl-mavlink-server            1.4.14
voxl-microdds-agent            3.0.0-0
voxl-modem                     1.2.3
voxl-mongoose                  7.19.0
voxl-mpa-to-ros                0.3.9
voxl-mpa-to-ros2               0.0.7
voxl-mpa-tools                 1.5.6
voxl-nano-tracker              0.1.7
voxl-open-vins-server          0.6.0
voxl-opencv                    4.5.5-3
voxl-osd                       0.3.8
voxl-portal                    0.8.7
voxl-px4                       1.14.0-2.0.133
voxl-px4-params                0.9.0
voxl-qvio-server               1.2.3
voxl-remote-id                 0.0.9
voxl-reset-slpi                0.0.1
voxl-ros2-foxy                 0.0.1
voxl-state-estimator           0.0.6
voxl-streamer                  0.8.0
voxl-suite                     1.6.3
voxl-tag-detector              0.1.0
voxl-tflite-server             0.5.1
voxl-utils                     2.0.2
voxl-uvc-server                0.1.7
voxl-vision-hub                1.9.21
voxl-vtx                       2.0.2
voxl-wavemux                   0.0.3
voxl2-io                       0.0.3
voxl2-security-hardening-utls  1.0-r0
voxl2-system-image             1.8.06-r0
voxl2-wlan                     1.0-r0
────────────────────────────────────────────────────────────────────────────────

Below are the full version details for SDK 1.5.0 (where the TOF camera worked as expected):

────────────────────────────────────────────────────────────────────────────────
system-image: 1.8.04-M0054-14.1a-perf
kernel:       #1 SMP PREEMPT Mon Mar 24 22:31:58 UTC 2025 4.19.125
────────────────────────────────────────────────────────────────────────────────
hw platform:  M0054
mach.var:     1.0.1
SKU:          MRB-D0012-4-V2-C29-T9-M28-X0
────────────────────────────────────────────────────────────────────────────────
voxl-suite:   1.5.0
────────────────────────────────────────────────────────────────────────────────
Packages:
Repo:  http://voxl-packages.modalai.com/ ./dists/qrb5165/sdk-1.5/binary-arm64/
Last Updated: 2026-02-05 12:12:50
List:
libfc-sensor                1.0.7
libmodal-cv                 0.5.18
libmodal-exposure           0.1.4
libmodal-journal            0.2.6
libmodal-json               0.4.7
libmodal-pipe               2.13.2
libqrb5165-io               0.5.0
libvoxl-cci-direct          0.3.3
libvoxl-cutils              0.1.5
modalai-slpi                1.2.0
mv-voxl                     0.1-r0
qrb5165-bind                0.1-r0
qrb5165-dfs-server          0.2.0
qrb5165-imu-server          1.1.3
qrb5165-mini-tof-server     0.2.2
qrb5165-rangefinder-server  0.1.5
qrb5165-slpi-test-sig       01-r0
qrb5165-system-tweaks       0.3.6
qrb5165-tflite              2.8.0-2
voxl-bind-spektrum          0.1.1
voxl-camera-calibration     0.6.0
voxl-camera-server          2.2.4
voxl-cassie-ros2            0.0.1
voxl-ceres-solver           2:1.14.0-10
voxl-configurator           1.0.2
voxl-cpu-monitor            0.6.0
voxl-cross-template         0.0.1
voxl-docker-support         1.3.1
voxl-elrs                   0.4.7
voxl-esc                    1.5.4
voxl-feature-tracker        0.5.2
voxl-flow-server            0.3.6
voxl-gphoto2-server         0.0.10
voxl-joystick-server        0.0.6
voxl-jpeg-turbo             2.1.3-7
voxl-lepton-server          1.3.3
voxl-lepton-tracker         0.0.4
voxl-libgphoto2             0.0.4
voxl-libuvc                 1.0.7
voxl-logger                 0.5.3
voxl-mavcam-manager         0.6.0
voxl-mavlink                0.1.5
voxl-mavlink-server         1.4.9
voxl-microdds-agent         3.0.0-0
voxl-modem                  1.1.8
voxl-mongoose               7.7.0-2
voxl-mpa-to-ros             0.3.9
voxl-mpa-tools              1.4.0
voxl-open-vins              0.4.19
voxl-open-vins-server       0.3.12
voxl-opencv                 4.5.5-3
voxl-osd                    0.1.8
voxl-portal                 0.7.11
voxl-px4                    1.14.0-2.0.105
voxl-px4-params             0.7.5
voxl-qvio-server            1.2.0
voxl-remote-id              0.0.9
voxl-reset-slpi             0.0.1
voxl-ros2-humble            0.0.3
voxl-state-estimator        0.0.5
voxl-streamer               0.7.5
voxl-suite                  1.5.0
voxl-tag-detector           0.0.5
voxl-tflite-server          0.4.1
voxl-utils                  1.4.8
voxl-uvc-server             0.1.7
voxl-vision-hub             1.8.23
voxl-vtx                    1.4.7
voxl-wavemux                0.0.1
voxl2-io                    0.0.3
voxl2-system-image          1.8.04-r0
voxl2-wlan                  1.0-r0
────────────────────────────────────────────────────────────────────────────────

Between versions 2.2.4 and 2.2.19 of the "voxl-camera-server" repository, I extracted the commits that modified the file "src/hal3_camera_mgr.cpp":

a7f5ea0 | 2025-12-10 | Eric Katzfey | Fixed the rest of the compiler warnings for the qrb5165-2 build
aa7b12d | 2025-12-05 | Alex Kushleyev | fix boson buffer pre-allocation size - the extra allocation should only be done in raw preview mode
f461dc0 | 2025-11-11 | Tom Martinson | Merge branch 'ar0144-rotate-encoded' into dev
3189b6b | 2025-11-10 | Alex Kushleyev | add option to invert boson output via a command
a3ac28e | 2025-11-10 | Tom Martinson | add new ar0144 rotated sensor type
bfce3c3 | 2025-11-10 | Alex Kushleyev | move some misp init code from camera mgr to misp
25fe9b6 | 2025-11-10 | Alex Kushleyev | load body to imu extrinsics from file
5289342 | 2025-11-07 | Alex Kushleyev | remove hist ae option; add manual and auto=lme_msv
a279e3d | 2025-11-06 | Jacob Camarillo | Remove unused OSD code
51cceec | 2025-11-06 | Alex Kushleyev | fix the neutral point of UV in monochrome YUVs from 127 to 128 for mono images sent to encoder
d1a9da8 | 2025-10-30 | Alex Kushleyev | Add an option to rotate AR0144 encoded video
86e1476 | 2025-10-29 | Alex Kushleyev | enable buffer recycling and camera customer register reinit for recovery
2b9816f | 2025-10-29 | Alex Kushleyev | Add boson 14bit support
cda94c2 | 2025-10-09 | zauberflote | Enable misp in stereo config
eeda888 | 2025-09-25 | Alex Kushleyev | enable rotation of imx412 and imx664 via en_rotate config option. using cci direct to set the readout direction
52643e8 | 2025-09-17 | Alex Kushleyev | fix bayer publish stride; enable publishing bayer images via shared ion buffers
2a2c14d | 2025-09-17 | Alex Kushleyev | Merge branch 'dev' into fix-encoder-dynamic-low-bitrate
dd7565a | 2025-09-16 | james | v2.2.7 remove voxl-cpu-monitor as build dependency and some general cleanups
a435e8c | 2025-09-08 | Alex Kushleyev | enable low fps to be set dynamically without having it set in the camera config (less than 1.5mbps for h264 and less than 3.0mbps for h265
272fe47 | 2025-09-03 | Alex Kushleyev | add a test colormap for boson using _color stream
1d81a46 | 2025-08-21 | Alex Kushleyev | fix pipe json for AR0144 Color from YUV to RGB
51afcb4 | 2025-08-21 | Alex Kushleyev | only read camera calibration if eis is enabled; add option to disable AWB on individual misp channels; add hooks for using RGB images instead of YUV for AWB
1c3e99e | 2025-08-14 | Alex Kushleyev | add imx214 misp supported resolutions
2390f20 | 2025-07-30 | modaljc | enable boson misp encoded by default instead of boson small encoded
50a27ba | 2025-07-30 | Alex Kushleyev | Merge branch 'dev' of gitlab.com:voxl-public/voxl-sdk/services/voxl-camera-server into dev
bbc9b89 | 2025-07-30 | Alex Kushleyev | add boson support to MISP; fix real-time fps control
96c6a0d | 2025-07-22 | james | v2.2.5 fix stereo cam pair going out of sync due to too high of an allowed discrepancy
055e11a | 2025-06-16 | Alex Kushleyev | clean up before merge to dev
effc3e8 | 2025-06-12 | Alex Kushleyev | add eis params and minor cleanup

@Alex-Kushleyev and modalai team, what can I do to fix that ?
Let me know if you need additional logs or tests.
Thanks in advence

Alex Kushleyev

@qt , there is a "decimator" param, which reduces the TOF fps in standby mode (when drone is not flying) in order to reduce the cpu usage, since the TOF processing software can use substantial cpu depending on the FPS. https://docs.modalai.com/voxl-camera-server/#camera-server-config-file

in your case, the decimator is set to 5, but i also see that "standby enabled" is set to false, so perhaps there is a bug that still applies the fps decimation.

Can you please try setting the decimator to 1 and see if that resolves your issue and we will double check if the standby_enabled parameter is not working correctly.

Alex

qt

@Alex-Kushleyev, thank you for your answer.
I confirm your assumption, when I set the decimator to 1, the fps is not divided.
Here are the tests I did on SDK 1.5.0 and SDK 1.6.3 :

test with SDK 1.5.0 ;

fps = 10 | standy_enabled = false or true | decimator = 5

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  2.90 |    0 |     1540 |     28.0  | 10.0 |    3.5 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
     7170622 | 240 | 180 |  7.1 | -0.0   0.0   0.0    0

fps = 60 | standy_enabled = false or true | decimator = 5

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  1.30 |    0 |     2155 |      9.3  | 59.9 |   20.7 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
    10666991 | 240 | 180 |  3.1 |  0.0   0.0   0.0    0

fps = 60 | standy_enabled = false or true | decimator = 5

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  1.18 |    0 |      869 |      9.4  | 59.9 |   20.7 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
    10753813 | 240 | 180 |  3.1 |  0.0   0.0   0.0    0

test with SDK 1.6.3

fps = 10 | standy_enabled = true or false | decimator = 1

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  3.02 |    0 |      260 |     29.7  | 10.0 |    3.5 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
     2052194 | 240 | 180 |  7.1 | -0.0   0.0   0.0    0

fps = 60 | standy_enabled = true or false | decimator = 1

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  1.18 |    0 |     7532 |      9.3  | 59.9 |   20.7 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
     2190721 | 240 | 180 |  3.1 |  0.0   0.0   0.0    0

fps = 60 | standy_enabled = true or false | decimator = 10

| Pipe Name |  bytes  | wide |  hgt |exp(ms)| gain | frame id |latency(ms)|  fps |  mbps  | format
| tof_depth |   43200 |  180 |  240 |  1.20 |    0 |       83 |     10.1  |  6.0 |    2.1 | RAW8

timestamp(ms)|  w  |  h  | Zmax | center point (m) (conf)
     2469720 | 240 | 180 |  3.1 |  0.0   0.0   0.0    0

As you can see, for SDK 1.5.0, fhe fps is NEVER divided, regardless of the value of standby_enabled.
For SDK 1.6.3, fhe fps is ALWAYS divided, regardless of the value of standby_enabled.
So, my problem is solved but I think you have a bug on the management of the paramater 'standby_enabled'.
I fastly investigate the code of voxl-camera-server and I can't find where you are using 'standby_enabled'. Here is the search result in all the project :

# Query: standby_en
# ContextLines: 2

6 results - 2 files

include/common_defs.h:
  370      modal_exposure_msv_config_t  ae_msv_info;  ///< ModalAI AE data (MSV)
  371  
  372:     int     standby_enabled;       ///< Standby enabled for lidar
  373      int     decimator;             ///< Decimator to use for standby
  374  

src/config_file.cpp:
  129          printf("    gain_min           : %d\n", cams[i].ae_msv_info.gain_min);
  130          printf("    gain_max           : %d\n", cams[i].ae_msv_info.gain_max);
  131:         printf("    standby_enabled:     %d\n", cams[i].standby_enabled);
  132          printf("    decimator:           %d\n", cams[i].decimator);
  133          printf("    independent_exposure:%d\n", cams[i].ind_exp);

  642          // standby settings for tof only
  643          if(is_tof_sensor(cam->type)) {
  644:             json_fetch_bool_with_default(item, "standby_enabled", (int*)&cam->standby_enabled, cam->standby_enabled);
  645              json_fetch_int_with_default  (item, "decimator", &cam->decimator,   cam->decimator);
  646          }

Maybe you should add a condition in your function PerCameraMgr::ProcessTOFPreviewFrame():

void PerCameraMgr::ProcessTOFPreviewFrame(mpa_ion_buf_t* buffer_info, camera_image_metadata_t meta)
{
    tofFrameCounter++;

    if(grab_cpu_pitmode_active() && tofFrameCounter % (int)configInfo.decimator != 0){
        return;
    }
    auto noStridePlaneSize = static_cast<size_t>(pre_width*pre_height*1.5);
    auto realWidth = static_cast<uint32_t>(pre_width*1.5);
    uint8_t* noStridePlane;
    if (buffer_info->stride != realWidth) {
        noStridePlane = new uint8_t[noStridePlaneSize];
        removePlaneStride(buffer_info->stride, realWidth, buffer_info->height,
                          (uint8_t*) buffer_info->vaddress, noStridePlane);
    }
    else {
        noStridePlane = static_cast<uint8_t*>(buffer_info->vaddress);
    }

    uint16_t srcPixel16[pre_width * pre_height] = {0};
    // NOTE we don't actually puvblish tis particular metadata to the pipe
    // TOF data is published separately in a very different way to cameras
    meta.format     = IMAGE_FORMAT_RAW8;
    meta.size_bytes = pre_width * pre_height;
    meta.stride     = pre_width;

    Mipi12ToRaw16(meta.size_bytes, noStridePlane, srcPixel16);
    tof_interface->ProcessRAW16(srcPixel16, meta.timestamp_ns);

    if (buffer_info->stride != realWidth) {
        delete[] noStridePlane;
    }
    M_VERBOSE("Sent tof data to royale for processing\n");

    return;
}

Best regards
Quentin