Send at least four S88 data octets per packets if possible

[model-railway-devices.git] / arducontrol / s88.c

diff --git a/arducontrol/s88.c b/arducontrol/s88.c
index 1ce574059fd848082d078e34033efeaa19abdd4d..bce0a2a090bc69ddea12eb0663962ee3cf32f900 100644 (file)
--- a/arducontrol/s88.c
+++ b/arducontrol/s88.c
@@ -28,14 +28,18 @@ void s88_init(void)
 
 void s88_check(void)
 {
-       // Only send one packet per check to avoid blocking
-       if(ringbuffer_fill(s88_buffer)>0)
+       uint8_t count = ringbuffer_fill(s88_buffer);
+       if(count>=4 || (count>0 && !s88_read_count))
        {
-               uint8_t reply[3];
+               uint8_t reply[10];
+               uint8_t i;
+
                reply[0] = S88_DATA;
-               reply[1] = s88_out_index++;
-               reply[2] = ringbuffer_pop(s88_buffer);
-               interface_send(reply, sizeof(reply));
+               reply[1] = s88_out_index;
+               for(i=0; i<count; ++i)
+                       reply[2+i] = ringbuffer_pop(s88_buffer);
+               interface_send(reply, 2+count);
+               s88_out_index += count;
        }
 }