MultiWii

Code: Select all

@@ -53,6 +54,8 @@
 #define MSP_SET_SERVO_CONF       212   //in message          Servo settings
 #define MSP_SET_MOTOR            214   //in message          PropBalance function

+#define MSP_TX_GPS_DATA          215   //in message          AGPS injection
+
 #define MSP_BIND                 240   //in message          no param

 #define MSP_EEPROM_WRITE         250   //in message          no param
@@ -71,7 +74,7 @@

@@ -79,7 +82,7 @@ static uint8_t cmdMSP[UART_NUMBER];

 void evaluateOtherData(uint8_t sr);
 #ifndef SUPPRESS_ALL_SERIAL_MSP
-void evaluateCommand();
+void evaluateCommand(uint8_t len);
 #endif

 #define BIND_CAPABLE 0;  //Used for Spektrum today; can be used in the future for any RX type that needs a bind and has a MultiWii
@@ -218,7 +221,7 @@ void serialCom() {
           inBuf[offset[CURRENTPORT]++][CURRENTPORT] = c;
         } else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] >= dataSize[CURRENTPORT]) {
           if (checksum[CURRENTPORT] == c) {  // compare calculated and transferred checksum
-            evaluateCommand();  // we got a valid packet, evaluate it
+            evaluateCommand(dataSize[CURRENTPORT]);  // we got a valid packet, evaluate it
           }
           c_state[CURRENTPORT] = IDLE;
           cc = 0; // no more than one MSP per port and per cycle
@@ -239,10 +242,20 @@ void s_struct_w(uint8_t *cb,uint8_t siz)
 }

 #ifndef SUPPRESS_ALL_SERIAL_MSP
-void evaluateCommand() {
+void evaluateCommand(uint8_t len) {
   uint32_t tmp=0;

   switch(cmdMSP[CURRENTPORT]) {
+
+#if GPS
+   case MSP_TX_GPS_DATA:
+     for (uint8_t i = 0; i < len; i++) {
+       SerialWrite(GPS_SERIAL, read8());
+       delay(5);
+     }
+     headSerialReply(0);
+     break;
+#endif
+
    case MSP_SET_RAW_RC:
      s_struct_w((uint8_t*)&rcSerial,16);
      rcSerialCount = 50; // 1s transition