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.