Inject IMU data into image data frame

[lida2003]

Hi

We are trying VINS-Fusion, which is for Non-GPS navigation.

Currently, it's quite good with Mono-Camera + IMU data. As most IPC camera is not that powerful, I want to move Fusion algorithm on the computer.

So I want to pack IMU data into image frame, which is about 60 or 120FPS.

Is it possible to add this into wfb-ng?

[svpcom]

yes, but you should do it on the camera (video encoder) side. CCTV cameras do this for metadata (for example to log motion detection) via adding NAL with invalid type which normal video decoder can skip. But for your case is more simple to mix it as prefix (or suffix) to RTP packets

[lida2003]

Currently, I'm using SSC338Q, which uses https://github.com/OpenIPC/majestic for encoding. And it's NOT opensource. Is there any alternative opensource code can be used (Or I can try to modify the code?), any performance impact?

[svpcom]

In case of openipc camera which is very restricted by disk size the more preferrable way is patch wfb_tx to mix IMU data to every incoming packet. See

wfb-ng/src/tx.cpp

Line 944 in 1ba87b2

struct iovec iov = { .iov_base = (void*)buf,

.
You need to add size of you IMU data struct to the buf size and set .iov_base = buf + sizeof(imu),

[svpcom]

On the rx side just write a simple proxy that will split prefixes from UDP packets and produce normal RTP and imu data streams

[lida2003]

You're a genius! I'll test it later. Thanks a lot. If it works, I'll submit a PR.

[svpcom]

But as long-term solution I'll recommend to write simple proxy in C that will receive mavlink from uart, udp from majectic then mix latest IMU data from mavlink as prefix to incoming rtp udp packet and send resulting udp packet to wfb_tx. It will have much simplier logic than hacking wfb_tx. See https://github.com/OpenIPC/mavfwd for reference

[lida2003]

Yes, I did some test on VINS-Fusion. Actually it's really time sensitive.
And I have no idea about image part below.

Now I'm thinking there are a few steps as follows:

Image part:（no idea right now??? how to get timestamp on image frame level(no experience here)）

1. get image time from camera
2. do image data time syn, line up with IPC system time
3. do image process lined up with IPC system time (NOT camera RTC/driver time)
4. wfb-ng tunnel transfer the image data

IMU part:

1. do FC time sync, line up with IPC system time
2. get IMU data with system time - time_offset
3. inject data to wfb-ng udp packet

[svpcom]

I think you don't need separate timestamp from image. Just use timestamp from flight controller (bundle it with IMU data). Because frequency of IMU readings >> video framerate they always will be correct. So proposed algorithm is:

wait for mavlink from /dev/ttySXX or RTP packet:
    if mavlink:
          timestamp = data
          imu = data
    if rtp:
        send_udp(timestamp, imu, rtp_data)

[lida2003]

There are some troubles here:

1. I looked into the format of RTP packets containing timestamps, and it seems that the RTP timestamp differs from what I understand as a system timestamp.

• RTP timestamps, something related to camera clock

// Define RTP header structure
typedef struct {
    // First byte: Version (2 bits), Padding (1 bit), Extension (1 bit), CSRC count (4 bits)
    uint8_t vpxcc;  
    
    // Second byte: Marker (1 bit), Payload Type (7 bits)
    uint8_t mpt;    
    
    // Sequence number (16 bits)
    uint16_t sequence_number;  
    
    // Timestamp (32 bits)
    uint32_t timestamp;  
    
    // SSRC (32 bits, synchronization source identifier)
    uint32_t ssrc;            
    
} rtp_header_t;

• system time from epoch

struct timespec {
    time_t tv_sec;   // Seconds since the Epoch (Jan 1, 1970)
    long   tv_nsec;  // Nanoseconds (1 second = 1,000,000,000 nanoseconds)
};

2. Additionally, if IMU data is injected into the image data stream, it would involve wfb-ng interacting with the application logic rather than merely functioning as an RF communication channel software.

3. I am not entirely sure when the start of a data frame occurs.

Therefore, I would like to confirm the following questions:

1. During the testing process of wfb-ng, does polling for data in the code indicate the start of a frame is avaliable?

wfb-ng/src/tx.cpp

Lines 932 to 935 in 7d9367a

	if (fds[i].revents & POLLIN)
	{
	uint8_t buf[MAX_PAYLOAD_SIZE + 1];
	uint8_t cmsgbuf[CMSG_SPACE(sizeof(uint32_t))];

2. And at that moment, does the UDP stream remain uninterrupted until the entire data is fully transmitted, continuously running in the for(;;) loop?

wfb-ng/src/tx.cpp

Line 940 in 7d9367a

for(;;)

If the above undestading is OK, then

1. Assuming the system time is obtained from the scenario in question 1. Then I can send this system timestamp to the backend server indicating a image frame is obtained at that moment.

2. Both the timestamp and IMU data are sent to the server through another wfb-ng channel, which just add a hook to wfb-ng for sending the timestamp to local imu agent. If there is an opensource image capture program, I think it will NOT be necessary to modify any of wfb-ng code. But right now, majestic is NOT opensouce.

3. The imu agent is mainly focused on sending timestamped imu data and image timestamp marker to back server using anohter wfb-ng tunnel. (Maybe a time sync alignment function can be implemented to see TOF between the agent and server.)

4. Then the server compare and match the image timestamp upon receiving the frame data? (Of course, certain anomalies such as packet loss might need to be handled.)

Please let me know what have got in your mind? Any advice? Thanks.

[svpcom]

If you bundle IMU data to the same UDP packet with video data on the TX side then on RX IMU and video will be always coherent. You don't need to add (or use) any timestamps to the video stream.

[ camera ] ---|mipi|--> [ majectic] ---| RTP | --> mixer ---> wfb_tx --- .... radio ... --> wfb_rx --> splitter -- |RTP|--> OpenCV
                 [flight controller] ---> mavlink----+                                                     +---|mavlink| ---+

where mixer and splitter are simple udp proxies.

If use separate streams or tunnels for video and mavlink then they will use different queues (in kernel, in card, etc) and always be out of sync.

[svpcom]

In approach above you don't need to modify to majestic and wfb-ng

[lida2003]

It's worth trying. And get back to you when I get result.

[lida2003]

@svpcom Does Majestic support to change RTP packet size.
Currently I found my test rtp source output 1400 bytes packet. And IPv4 IP(20)+UDP(8)+payload, 1500 - 20 - 8 = 1472, only 72 extra for IMU/time.

What would happen if I send UDP packet size beyond or large than MTU size(1500)? Any trouble with wfb-ng?Will the underlying layer perform fragmentation?

[svpcom]

@lida2003

1. In /etc/majestic.yaml you can set rtp packet size
2. For 8812au/eu with latest drivers mtu (max udp packet size) is 3993 but see note

   wfb-ng/src/wifibroadcast.hpp

   Line 103 in 4ea7006

   #define WIFI_MTU 4045 // Max injected packet size including all wfb-headers.

If you will try to inject packet larger than wifi mtu then it will be truncated and you will unable to decrypt it on the RX side.
Also all mtu > 1500 may have interoperability issues with non 8812au/eu cards (for example with ath9k)

[lida2003]

OK, that's good news. I will switch to openipc camera(SSC338Q) and find out more.

And I have found there are glitches on Rpi3B+, I'm NOT sure why?

• image glitch when using Rpib+ RTP source #1840

[lida2003]

I have got an error(Error: Unable to inject packet: Message too long) on openipc-ssc30kq IMX335 8812EU.

How to configure to use large MTU?

• UDP send 1215+48*5+16=1487+28=1515 NG
• UDP send 1215+48*3+16=1375+28=1403 OK

RTW: module init start
RTW: rtl88x2eu v5.15.0.1-186-g768722062.20230815_COEX20230616-330a
RTW: build time: Apr 10 2024 12:55:07
RTW: rtl88x2eu BT-Coex version = COEX20230616-330a
RTW: rtw_inetaddr_notifier_register

# wfb_tx -p 0 -u 5600 -R 456000 -K /etc/drone.key -B 20 -M 1 -S 1 -L 1 -G long -k 8 -n 12 -T 0 -i 7669206 -f data -C 8000 wlan0
Using data frames
Listen on 5600 for wlan0
Listen on 8000 for management commands
169142 PKT     0:0:0:0:0:0:0
170142 TX_ANT  ff      627:0:29:47:278
170142 PKT     0:418:499658:627:788490:0:0
Error: Unable to inject packet: Message too long

[svpcom]

@lida2003 ifconfig wlan0 mtu 3000

[lida2003]

@lida2003 ifconfig wlan0 mtu 3000

OK, I'll try if needed. Right now I didn't see that much IMU packet needed for transfer.

• 100Hz ACC&gyro data acq freq
• Each RTP can carry 3 imu structure(48B)
• Each frame takes about least 11 RTP packets
• image@FPS30, there will be 30x11x3=990 packets >>100Hz

But it seems still have sync issue: HKUST-Aerial-Robotics/VINS-Fusion#263
Hmm... Not sure what to do next....