AP_Frsky_Telem: fixes #5862

MAVLink strings of exactly 50 characters are not NULL terminated, so
instead we must check that the 50th character has been reached.

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -421,32 +421,25 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
         return false;
     }
 
-    if (_msg_chunk.repeats == 0) {
-        _msg_chunk.chunk = 0;
-        uint8_t character = _statustext_queue[0]->text[_msg_chunk.char_index++];
-        if (character) {
-            _msg_chunk.chunk |= character<<24;
+    if (_msg_chunk.repeats == 0) { // if it's the first time get_next_msg_chunk is called for a given chunk
+        uint8_t character;
+        _msg_chunk.chunk = 0; // clear the 4 bytes of the chunk buffer
+        for(int i = 3; i > -1 && _msg_chunk.char_index < sizeof(_statustext_queue[0]->text); i--) {
             character = _statustext_queue[0]->text[_msg_chunk.char_index++];
-            if (character) {
-                _msg_chunk.chunk |= character<<16;
-                character = _statustext_queue[0]->text[_msg_chunk.char_index++];
-                if (character) {
-                    _msg_chunk.chunk |= character<<8;
-                    character = _statustext_queue[0]->text[_msg_chunk.char_index++];
-                    if (character) {
-                        _msg_chunk.chunk |= character;
-                    }
-                }
+            if (!character) {
+                break;
             }
-        }
-        if (!character) { // we've reached the end of the message (string terminated by '\0')
-            _msg_chunk.char_index = 0;
+            _msg_chunk.chunk |= character << i * 8;
+        }        
+        if ((!character)||(_msg_chunk.char_index == sizeof(_statustext_queue[0]->text))) { // we've reached the end of the message (string terminated by '\0' or last character of the string has been processed)
+            _msg_chunk.char_index = 0; // reset index to get ready to process the next message
             // add severity which is sent as the MSB of the last three bytes of the last chunk (bits 24, 16, and 8) since a character is on 7 bits
             _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x4)<<21;
             _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x2)<<14;
             _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x1)<<7;
         }
     }
+    
     _msg_chunk.repeats++;
     if (_msg_chunk.repeats > 2) { // repeat each message chunk 3 times to ensure transmission
         _msg_chunk.repeats = 0;