Add some debug messages to SITL

Great to diagnose your msp messages are actually being delivered. :)

Author: mmosca

src/main/fc/fc_msp.c

@@ -18,6 +18,7 @@
 #include <ctype.h>
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <string.h>
 #include <math.h>
 
@@ -3290,9 +3291,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #endif
     case MSP2_INAV_GPS_UBLOX_COMMAND:
         if(dataSize < 8 || !isGpsUblox()) {
+            SD(fprintf(stderr, "[GPS] Not ublox!\n"));
             return MSP_RESULT_ERROR;
         }
 
+        SD(fprintf(stderr, "[GPS] Sending ubx command: %i!\n", dataSize));
         gpsUbloxSendCommand(src->ptr, dataSize, 0);
         break;
 
@@ -4142,6 +4145,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
     // initialize reply by default
     reply->cmd = cmd->cmd;
 
+    SD(fprintf(stderr, "[MSP] CommandId: 0x%04x bytes: %i!\n", cmdMSP, sbufBytesRemaining(src)));
     if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) {
         ret = mspProcessSensorCommand(cmdMSP, src);
     } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {

src/main/msp/msp_serial.c

@@ -17,6 +17,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdio.h>
+#include <ctype.h>
 #include <string.h>
 
 #include "platform.h"
@@ -171,8 +173,10 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
         case MSP_CHECKSUM_V1:
             if (mspPort->checksum1 == c) {
                 mspPort->c_state = MSP_COMMAND_RECEIVED;
+                SD(fprintf(stderr, "[MSPV1] Command received\n"));
             } else {
                 mspPort->c_state = MSP_IDLE;
+                SD(fprintf(stderr, "[MSPV1] Checksum error!\n"));
             }
             break;
 
@@ -225,6 +229,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
                 // Check for potential buffer overflow
                 if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
                     mspPort->c_state = MSP_IDLE;
+                    SD(fprintf(stderr, "[MSPV2] Potential buffer overflow!\n"));
                 }
                 else {
                     mspPort->dataSize = hdrv2->size;
@@ -248,7 +253,9 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
         case MSP_CHECKSUM_V2_NATIVE:
             if (mspPort->checksum2 == c) {
                 mspPort->c_state = MSP_COMMAND_RECEIVED;
+                SD(fprintf(stderr, "[MSPV2] command received!\n"));
             } else {
+                SD(fprintf(stderr, "[MSPV2] Checksum error!\n"));
                 mspPort->c_state = MSP_IDLE;
             }
             break;
@@ -472,6 +479,7 @@ void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e
             const uint8_t c = serialRead(mspPort->port);
             const bool consumed = mspSerialProcessReceivedData(mspPort, c);
 
+            //SD(fprintf(stderr, "[MSP]: received char: %02x (%c) state: %i\n", c, isprint(c) ? c : '.', mspPort->c_state));
             if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
                 mspEvaluateNonMspData(mspPort, c);
             }