AP_Frsky_Telem: fix for passthrough telemetry stall while sending message chunks

This prevents the library from giving message chunks a too high priority leading to
all telemetry packets but 0x5006(attitude) to starve.

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -95,6 +95,20 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
     }
 
     if ((prev_byte == START_STOP_SPORT) && (_passthrough.new_byte == SENSOR_ID_28)) { // byte 0x7E is the header of each poll request
+        if (_passthrough.send_chunk) { // skip other data and send a message chunk during this iteration
+            _passthrough.send_chunk = false;
+            if (get_next_msg_chunk()) {
+                send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
+            }
+        } else {
+            // build message queue for sensor_status_flags
+            check_sensor_status_flags();
+            // build message queue for ekf_status
+            check_ekf_status();
+            // if there are pending messages in the queue, send them during next iteration
+            if (!_statustext_queue.empty()) {
+                _passthrough.send_chunk = true;
+            }
             if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration
                 _passthrough.send_attiandrng = false; // next iteration, check if we should send something other
             } else { // send other sensor data if it's time for them, and reset the corresponding timer if sent
@@ -105,15 +119,6 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
                     _passthrough.params_timer = AP_HAL::millis();
                     return;
                 }
-            // build message queue for sensor_status_flags
-            check_sensor_status_flags();
-            // build message queue for ekf_status
-            check_ekf_status();
-            // if there's any message in the queue, start sending them chunk by chunk; three times each chunk
-            if (get_next_msg_chunk()) {
-                send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
-                return;
-            }
                 if ((now - _passthrough.ap_status_timer) >= 500) {
                     if (gcs().vehicle_initialised()) {  // send ap status only once vehicle has been initialised
                         send_uint32(DIY_FIRST_ID+1, calc_ap_status());
@@ -160,6 +165,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
             send_uint32(DIY_FIRST_ID+6, calc_attiandrng());
         }
     }
+}
 
 /*
  * send telemetry data

libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@@ -162,6 +162,7 @@ private:
         uint8_t new_byte;
         bool send_attiandrng;
         bool send_latitude;
+        bool send_chunk;
         uint32_t params_timer;
         uint32_t ap_status_timer;
         uint32_t batt_timer;