AP_Frsky_Telem: increase sending params priority

If there's a lot of messages in the queue, params would not be
transmitted for a while, until the queue is empty, which can take a bit
of time during init.

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -94,8 +94,14 @@ 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_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 { // check if there's other data to send
+        } else { // send other sensor data if it's time for them, and reset the corresponding timer if sent
             _passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth
+            uint32_t now = AP_HAL::millis();
+            if (((now - _passthrough.params_timer) > 1000) && (!AP_Notify::flags.armed)) {
+                send_uint32(DIY_FIRST_ID+7, calc_param());
+                _passthrough.params_timer = AP_HAL::millis();
+                return;
+            }
             // build message queue for sensor_status_flags
             check_sensor_status_flags();
             // build message queue for ekf_status
@@ -105,35 +111,34 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
                 send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
                 return;
             }
-            // send other sensor data if it's time for them, and reset the corresponding timer if sent
-            uint32_t now = AP_HAL::millis();
-            if (((now - _passthrough.params_timer) > 1000) && (!AP_Notify::flags.armed)) {
-                send_uint32(DIY_FIRST_ID+7, calc_param());
-                _passthrough.params_timer = AP_HAL::millis();
-                return;
-            } else if ((now - _passthrough.ap_status_timer) > 500) {
+            if ((now - _passthrough.ap_status_timer) > 500) {
                 if (((*_ap.valuep) & AP_INITIALIZED_FLAG) > 0) {  // send ap status only once vehicle has been initialised
                     send_uint32(DIY_FIRST_ID+1, calc_ap_status());
                     _passthrough.ap_status_timer = AP_HAL::millis();
                 }
                 return;
-            } else if ((now - _passthrough.batt_timer) > 1000) {
+            }
+            if ((now - _passthrough.batt_timer) > 1000) {
                 send_uint32(DIY_FIRST_ID+3, calc_batt());
                 _passthrough.batt_timer = AP_HAL::millis();
                 return;
-            } else if ((now - _passthrough.gps_status_timer) > 1000) {
+            }
+            if ((now - _passthrough.gps_status_timer) > 1000) {
                 send_uint32(DIY_FIRST_ID+2, calc_gps_status());
                 _passthrough.gps_status_timer = AP_HAL::millis();
                 return;
-            } else if ((now - _passthrough.home_timer) > 500) {
+            }
+            if ((now - _passthrough.home_timer) > 500) {
                 send_uint32(DIY_FIRST_ID+4, calc_home());
                 _passthrough.home_timer = AP_HAL::millis();
                 return;
-            } else if ((now - _passthrough.velandyaw_timer) > 500) {
+            }
+            if ((now - _passthrough.velandyaw_timer) > 500) {
                 send_uint32(DIY_FIRST_ID+5, calc_velandyaw());
                 _passthrough.velandyaw_timer = AP_HAL::millis();
                 return;
-            } else if ((now - _passthrough.gps_latlng_timer) > 1000) {
+            }
+            if ((now - _passthrough.gps_latlng_timer) > 1000) {
                 send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
                 if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer
                     _passthrough.gps_latlng_timer = AP_HAL::millis();