Merge remote-tracking branch 'origin/master' into mmosca-sbus2-telemetry

docs/Settings.md

@@ -2518,7 +2518,7 @@ Rate of the extra1 message for MAVLink telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 10 | 0 | 255 |
+| 2 | 0 | 255 |
 
 ---
 
@@ -2558,7 +2558,7 @@ Rate of the RC channels message for MAVLink telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 5 | 0 | 255 |
+| 1 | 0 | 255 |
 
 ---
 

src/main/blackbox/blackbox.c

@@ -1442,6 +1442,9 @@ static void blackboxValidateConfig(void)
 #endif
 #ifdef USE_SDCARD
     case BLACKBOX_DEVICE_SDCARD:
+#endif
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
 #endif
     case BLACKBOX_DEVICE_SERIAL:
         // Device supported, leave the setting alone

src/main/blackbox/blackbox_io.c

@@ -19,6 +19,11 @@
 #include <stdarg.h>
 #include <string.h>
 
+#if defined(SITL_BUILD)
+#include <stdio.h>
+#include <time.h>
+#endif
+
 #include "platform.h"
 
 #ifdef USE_BLACKBOX
@@ -81,6 +86,12 @@ static struct {
 
 #endif
 
+#if defined(SITL_BUILD)
+static struct {
+    FILE *file_handler;
+} blackboxFile;
+#endif
+
 #ifndef UNIT_TEST
 void blackboxOpen(void)
 {
@@ -103,6 +114,11 @@ void blackboxWrite(uint8_t value)
     case BLACKBOX_DEVICE_SDCARD:
         afatfs_fputc(blackboxSDCard.logFile, value);
         break;
+#endif
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        fputc(value, blackboxFile.file_handler);
+        break;
 #endif
     case BLACKBOX_DEVICE_SERIAL:
     default:
@@ -133,6 +149,13 @@ int blackboxPrint(const char *s)
         break;
 #endif
 
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        length = strlen(s);
+        fputs(s, blackboxFile.file_handler);
+        break;
+#endif
+
     case BLACKBOX_DEVICE_SERIAL:
     default:
         pos = (uint8_t*) s;
@@ -196,6 +219,12 @@ bool blackboxDeviceFlushForce(void)
         return afatfs_flush();
 #endif
 
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        fflush(blackboxFile.file_handler);
+        return true;
+#endif
+
     default:
         return false;
     }
@@ -271,6 +300,26 @@ bool blackboxDeviceOpen(void)
         return true;
         break;
 #endif
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        {
+            const time_t now = time(NULL);
+            const struct tm *t = localtime(&now);
+            char filename[32];
+            strftime(filename, sizeof(filename), "%Y_%m_%d_%H%M%S.TXT", t);
+
+            blackboxFile.file_handler = fopen(filename, "wb");
+            if (blackboxFile.file_handler == NULL) {
+                fprintf(stderr, "[BlackBox] Failed to create log file\n");
+                return false;
+            }
+            fprintf(stderr, "[BlackBox] Created %s\n", filename);
+        }
+
+        blackboxMaxHeaderBytesPerIteration = BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION;
+        return true;
+        break;
+#endif
     default:
         return false;
     }
@@ -302,6 +351,12 @@ void blackboxDeviceClose(void)
         // Some flash device, e.g., NAND devices, require explicit close to flush internally buffered data.
         flashfsClose();
         break;
+#endif
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        fclose(blackboxFile.file_handler);
+        blackboxFile.file_handler = NULL;
+        break;
 #endif
     default:
         ;
@@ -506,6 +561,11 @@ bool isBlackboxDeviceFull(void)
         return afatfs_isFull();
 #endif
 
+#if defined (SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        return false;
+#endif
+
     default:
         return false;
     }
@@ -523,6 +583,10 @@ bool isBlackboxDeviceWorking(void)
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
             return flashfsIsReady();
+#endif
+#if defined(SITL_BUILD)
+        case BLACKBOX_DEVICE_FILE:
+            return blackboxFile.file_handler != NULL;
 #endif
     default:
         return false;
@@ -562,6 +626,11 @@ void blackboxReplenishHeaderBudget(void)
     case BLACKBOX_DEVICE_SDCARD:
         freeSpace = afatfs_getFreeBufferSpace();
         break;
+#endif
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        freeSpace = BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET;
+        break;
 #endif
     default:
         freeSpace = 0;
@@ -631,6 +700,12 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
         return BLACKBOX_RESERVE_TEMPORARY_FAILURE;
 #endif
 
+#if defined(SITL_BUILD)
+    case BLACKBOX_DEVICE_FILE:
+        // Assume that all writes will fit in the file's buffers
+        return BLACKBOX_RESERVE_TEMPORARY_FAILURE;
+#endif
+
     default:
         return BLACKBOX_RESERVE_PERMANENT_FAILURE;
     }

src/main/blackbox/blackbox_io.h

@@ -31,6 +31,9 @@ typedef enum BlackboxDevice {
 #ifdef USE_SDCARD
     BLACKBOX_DEVICE_SDCARD = 2,
 #endif
+#if defined(SITL_BUILD)
+    BLACKBOX_DEVICE_FILE = 3,
+#endif
 
     BLACKBOX_DEVICE_END
 } BlackboxDevice;

src/main/fc/settings.yaml

@@ -25,7 +25,7 @@ tables:
   - name: serial_rx
     values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS", "SBUS2"]
   - name: blackbox_device
-    values: ["SERIAL", "SPIFLASH", "SDCARD"]
+    values: ["SERIAL", "SPIFLASH", "SDCARD", "FILE"]
   - name: motor_pwm_protocol
     values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"]
   - name: servo_protocol
@@ -3155,7 +3155,7 @@ groups:
         type: uint8_t
         min: 0
         max: 255
-        default_value: 5
+        default_value: 1
       - name: mavlink_pos_rate
         description: "Rate of the position message for MAVLink telemetry"
         field: mavlink.position_rate
@@ -3169,7 +3169,7 @@ groups:
         type: uint8_t
         min: 0
         max: 255
-        default_value: 10
+        default_value: 2
       - name: mavlink_extra2_rate
         description: "Rate of the extra2 message for MAVLink telemetry"
         field: mavlink.extra2_rate

src/main/target/GEPRC_F722_AIO/target.h

@@ -108,6 +108,10 @@
 #define UART2_RX_PIN            PA3
 #define UART2_TX_PIN            PA2
 
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
 #define USE_UART4
 #define UART4_RX_PIN            PC11
 #define UART4_TX_PIN            PC10
@@ -116,7 +120,7 @@
 #define UART5_RX_PIN            PD2
 #define UART5_TX_PIN            PC12
 
-#define SERIAL_PORT_COUNT       5
+#define SERIAL_PORT_COUNT       6
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS

src/main/telemetry/mavlink.c

@@ -162,14 +162,16 @@ static serialPortConfig_t *portConfig;
 
 static bool mavlinkTelemetryEnabled =  false;
 static portSharing_e mavlinkPortSharing;
+static uint8_t txbuff_free = 100;
+static bool txbuff_valid = false;
 
 /* MAVLink datastream rates in Hz */
 static uint8_t mavRates[] = {
     [MAV_DATA_STREAM_EXTENDED_STATUS] = 2,      // 2Hz
-    [MAV_DATA_STREAM_RC_CHANNELS] = 5,          // 5Hz
+    [MAV_DATA_STREAM_RC_CHANNELS] = 1,          // 1Hz
     [MAV_DATA_STREAM_POSITION] = 2,             // 2Hz
-    [MAV_DATA_STREAM_EXTRA1] = 10,              // 10Hz
-    [MAV_DATA_STREAM_EXTRA2] = 2,               // 2Hz
+    [MAV_DATA_STREAM_EXTRA1] = 3,               // 3Hz
+    [MAV_DATA_STREAM_EXTRA2] = 2,               // 2Hz, HEARTBEATs are important
     [MAV_DATA_STREAM_EXTRA3] = 1                // 1Hz
 };
 
@@ -1111,6 +1113,27 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
     return true;
 }
 
+static bool handleIncoming_PARAM_REQUEST_LIST(void) {
+    mavlink_param_request_list_t msg;
+    mavlink_msg_param_request_list_decode(&mavRecvMsg, &msg);
+
+    // Respond that we don't have any parameters to force Mission Planner to give up quickly
+    if (msg.target_system == mavSystemId) {
+        // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index);
+        mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0);
+        mavlinkSendMessage();
+    }
+    return true;
+}
+
+static bool handleIncoming_RADIO_STATUS(void) {
+    mavlink_radio_status_t msg;
+    mavlink_msg_radio_status_decode(&mavRecvMsg, &msg);
+    txbuff_valid = true;
+    txbuff_free = msg.txbuf;
+    return true;
+}
+
 #ifdef USE_ADSB
 static bool handleIncoming_ADSB_VEHICLE(void) {
     mavlink_adsb_vehicle_t msg;
@@ -1155,6 +1178,7 @@ static bool handleIncoming_ADSB_VEHICLE(void) {
 }
 #endif
 
+// Returns whether a message was processed
 static bool processMAVLinkIncomingTelemetry(void)
 {
     while (serialRxBytesWaiting(mavlinkPort) > 0) {
@@ -1165,6 +1189,8 @@ static bool processMAVLinkIncomingTelemetry(void)
             switch (mavRecvMsg.msgid) {
                 case MAVLINK_MSG_ID_HEARTBEAT:
                     break;
+                case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+                    return handleIncoming_PARAM_REQUEST_LIST();
                 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
                     return handleIncoming_MISSION_CLEAR_ALL();
                 case MAVLINK_MSG_ID_MISSION_COUNT:
@@ -1176,11 +1202,17 @@ static bool processMAVLinkIncomingTelemetry(void)
                 case MAVLINK_MSG_ID_MISSION_REQUEST:
                     return handleIncoming_MISSION_REQUEST();
                 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
-                    return handleIncoming_RC_CHANNELS_OVERRIDE();
+                    handleIncoming_RC_CHANNELS_OVERRIDE();
+                    // Don't set that we handled a message, otherwise RC channel packets will block telemetry messages
+                    return false;
 #ifdef USE_ADSB
                 case MAVLINK_MSG_ID_ADSB_VEHICLE:
                     return handleIncoming_ADSB_VEHICLE();
 #endif
+                case MAVLINK_MSG_ID_RADIO_STATUS:
+                    handleIncoming_RADIO_STATUS();
+                    // Don't set that we handled a message, otherwise radio status packets will block telemetry messages
+                    return false;
                 default:
                     return false;
             }
@@ -1190,10 +1222,13 @@ static bool processMAVLinkIncomingTelemetry(void)
     return false;
 }
 
+static bool isMAVLinkTelemetryHalfDuplex(void) {
+    return telemetryConfig()->halfDuplex ||
+            (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex));
+}
+
 void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
 {
-    static bool incomingRequestServed;
-
     if (!mavlinkTelemetryEnabled) {
         return;
     }
@@ -1202,24 +1237,23 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
         return;
     }
 
-    // If we did serve data on incoming request - skip next scheduled messages batch to avoid link clogging
-    if (processMAVLinkIncomingTelemetry()) {
-        incomingRequestServed = true;
+    // Process incoming MAVLink
+    bool receivedMessage = processMAVLinkIncomingTelemetry();
+    bool shouldSendTelemetry = false;
+
+    // Determine whether to send telemetry back based on flow control / pacing
+    if (txbuff_valid) {
+        // Use flow control if available
+        shouldSendTelemetry = txbuff_free >= 33;
+    } else {
+        // If not, use blind frame pacing - and back off for collision avoidance if half-duplex
+        bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage);
+        shouldSendTelemetry = ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff;
     }
 
-    if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
-        // Only process scheduled data if we didn't serve any incoming request this cycle
-        if (!incomingRequestServed ||
-            (
-                 (rxConfig()->receiverType == RX_TYPE_SERIAL) &&
-                 (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
-                 !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
-            )
-        ) {
-            processMAVLinkTelemetry(currentTimeUs);
-        }
+    if (shouldSendTelemetry) {
+        processMAVLinkTelemetry(currentTimeUs);
         lastMavlinkMessage = currentTimeUs;
-        incomingRequestServed = false;
     }
 }