Add msp command to send raw ublox commands to gps

.gitignore

@@ -40,4 +40,9 @@ make/local.mk
 launch.json
 .vscode/tasks.json
 .vscode/c_cpp_properties.json
+
 /cmake-build-debug/
+
+# Assitnow token and files for test script
+tokens.yaml
+*.ubx

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>
 
@@ -87,6 +88,7 @@
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
+#include "io/gps_ublox.h"
 #include "io/opflow.h"
 #include "io/rangefinder.h"
 #include "io/ledstrip.h"
@@ -3287,6 +3289,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 #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;
 
 #ifdef USE_EZ_TUNE
 
@@ -4134,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/io/gps_ublox.c

@@ -128,7 +128,7 @@ static struct {
 
 
 // Send buffer
-static union {
+static union send_buffer_t {
     ubx_message message;
     uint8_t bytes[58];
 } send_buffer;
@@ -212,6 +212,20 @@ static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
     return GPS_NO_FIX;
 }
 
+bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t timeout)
+{
+    UNUSED(timeout);
+
+    serialWriteBuf(gpsState.gpsPort, rawCommand, commandLen);
+
+    union send_buffer_t *sb = (union send_buffer_t *)(rawCommand);
+
+    _ack_waiting_msg = sb->message.header.msg_id;
+    _ack_state = UBX_ACK_WAITING;
+
+    return true;
+}
+
 static void sendConfigMessageUBLOX(void)
 {
     uint8_t ck_a=0, ck_b=0;
@@ -1118,4 +1132,13 @@ void gpsHandleUBLOX(void)
     }
 }
 
+bool isGpsUblox(void)
+{
+    if(gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) {
+        return true;
+    }
+
+    return false;
+}
+
 #endif

src/main/io/gps_ublox.h

@@ -424,6 +424,9 @@ bool gpsUbloxGlonassDefault(void);
 bool gpsUbloxGalileoEnabled(void);
 bool gpsUbloxBeidouEnabled(void);
 bool gpsUbloxGlonassEnabled(void);
+bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t timeout);
+
+bool isGpsUblox(void);
 
 
 #ifdef __cplusplus

src/main/msp/msp_protocol_v2_inav.h

@@ -96,6 +96,8 @@
 #define MSP2_INAV_FW_APPROACH                   0x204A
 #define MSP2_INAV_SET_FW_APPROACH               0x204B
 
+#define MSP2_INAV_GPS_UBLOX_COMMAND             0x2050
+
 #define MSP2_INAV_RATE_DYNAMICS                 0x2060
 #define MSP2_INAV_SET_RATE_DYNAMICS             0x2061
 

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);
             }