Hello Victor. As I said in my private mail to you the code does'nt work between 2 mega arduinos. I am using Serial3 to connect them on each board.
I had to redo my own protocol and had similar issue until I discovered that the process time in a loop is so much higher than the Serial frequency that when asking for a Serial3.read the result is often irevelant/wrong : the read has not yet received all the bytes. So I corrected the issue by checking when read gives sufficient bytes to read.
I'll test a solution on your protocol during the coming days and will post my feedback the same time I'll answer your request.
Regards and thank you for the job
Emmanuel