first pass at working mavlink forwarding

stronnag · Feb 5, 2018 · bcdcbab · bcdcbab
1 parent 4e60683
commit bcdcbab
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 56 deletions.
diff --git a/common/serial-device.vala b/common/serial-device.vala
@@ -1014,35 +1014,42 @@ public class MWSerial : Object
         }
     }
 
-    public void send_mav(uint8 cmd, void *data, size_t len)
+
+    public void send_mav(uint8 cmd, uint8[]data, size_t len)
     {
+        const uint8 MAVID1='j';
+        const uint8 MAVID2='h';
+
         if(available == true)
         {
             uint16 mcrc;
 
             check_txbuf_size(len+8);
-            uint8 * tx = txbuf;
-            *tx++ = 0xfe;
-            *tx++ = (uint8)len;
-            *tx++ = mavseqno++; // overflow is fine;
-            *tx++ = 'M';
-            *tx++ = 'P';
-            *tx++ = cmd;
-            if (data != null && len > 0)
-                Posix.memcpy(tx, data, len);
-            tx = tx+len;
-
-            mcrc = mavlink_crc(0xffff, (uint8)len);
-            for(var i = 2; i < len + 4; i++)
-                mcrc = mavlink_crc(0xffff, txbuf[i]);
+            txbuf[0] = 0xfe;
+            txbuf[1] = (uint8)len;
+            mcrc = mavlink_crc(0xffff, txbuf[1]);
+            txbuf[2] = mavseqno++; // overflow is fine;
+
+            mcrc = mavlink_crc(mcrc, txbuf[2]);
+            txbuf[3] = MAVID1;
+            mcrc = mavlink_crc(mcrc, txbuf[3]);
+            txbuf[4] = MAVID2;
+            mcrc = mavlink_crc(mcrc, txbuf[4]);
+            txbuf[5] = cmd;
+            mcrc = mavlink_crc(mcrc, txbuf[5]);
+            for(var j = 0; j < len; j++)
+            {
+                uint8 c = data[j];
+                txbuf[6+j] = c;
+                mcrc = mavlink_crc(mcrc, c);
+            }
             mcrc = mavlink_crc(mcrc, MAVCRCS[cmd]);
-            *tx++ = (uint8)(mcrc&0xff);
-            *tx++ = (uint8)(mcrc >> 8);
+            txbuf[len+6] = (uint8)(mcrc&0xff);
+            txbuf[len+7] = (uint8)(mcrc >> 8);
             write(txbuf, (len+8));
         }
     }
 
-
     private size_t generate_v1(uint8 cmd, void *data, size_t len)
     {
         uint8 ck = 0;

diff --git a/mwp/mwp.vala b/mwp/mwp.vala
@@ -5005,32 +5005,8 @@ public class MWPlanner : Gtk.Application {
 
     private void send_mav_heartbeat()
     {
-        uint8 mbuf[32];
-        mbuf[0] = 0xfe;
-        mbuf[1] = 9; // size
-        mbuf[2] = 0;
-        mbuf[3] = 0;
-        mbuf[4] = 0;
-        mbuf[5] = 0;
-        for(var j =0; j < 9; j++)
-            mbuf[6+j] = 0;
-
-        uint8 length = mbuf[1];
-        uint16 sum = 0xFFFF;
-        uint8 i, stoplen;
-        stoplen = length + 6;
-        mbuf[length+6] = 50;
-        stoplen++;
-
-        i = 1;
-        while (i<stoplen)
-        {
-            sum = msp.mavlink_crc(sum, mbuf[i]);
-            i++;
-        }
-        mbuf[length+6] = (uint8)sum&0xFF;
-        mbuf[length+7] = sum>>8;
-        msp.write(mbuf,length+8);
+        uint8 dummy[9]={0};
+        msp.send_mav(0, dummy, 9);
     }
 
     private void report_bits(uint64 bits)
@@ -5549,6 +5525,18 @@ public class MWPlanner : Gtk.Application {
             var serdev = dev_entry.get_active_text();
             string estr;
             serstate = SERSTATE.NONE;
+            if(forward_device != null)
+            {
+                string fstr;
+                if(fwddev.open(forward_device, 0, out fstr) == true)
+                {
+                    fwddev.set_mode(MWSerial.Mode.SIM);
+                    MWPLog.message("set forwarder %s\n", forward_device);
+                }
+                else
+                    MWPLog.message("Forwarder %s %s\n", forward_device, fstr);
+            }
+
             if (msp.open(serdev, conf.baudrate, out estr) == true)
             {
                 xarm_flags=0xffff;
@@ -5568,17 +5556,6 @@ public class MWPlanner : Gtk.Application {
                     queue_cmd(MSP.Cmds.IDENT,null,0);
                     run_queue();
                 }
-                if(forward_device != null)
-                {
-                    string fstr;
-                    if(fwddev.open(forward_device, 0, out fstr) == true)
-                    {
-                        fwddev.set_mode(MWSerial.Mode.SIM);
-                        MWPLog.message("set forwarder %s\n", forward_device);
-                    }
-                    else
-                        MWPLog.message("Forwarder %s %s\n", forward_device, fstr);
-                }
             }
             else
             {

diff --git a/mwp/mwpid.h b/mwp/mwpid.h
@@ -1 +1 @@
-char * mwpid="1.036.556";
+char * mwpid="1.036.603";
Original file line number	Diff line number	Diff line change
		@@ -1 +1 @@
		char * mwpid="1.036.556";
		char * mwpid="1.036.603";