Diff: main.cpp

Revision:

22:9880a26886db

Parent:

21:c546eab07e28

Child:

23:497f8faa908e

--- a/main.cpp	Mon Apr 21 13:54:55 2014 +0000
+++ b/main.cpp	Tue Apr 22 00:10:16 2014 +0000
@@ -112,7 +112,14 @@
     uint16_t count; 
 }MAV_COUNT;
 
-typedef struct mav_APM_S{
+typedef struct MAV_REQ_S{
+    char targSys;
+    char targComp;
+    char other;
+    char count; 
+}MAV_REQ;
+
+typedef struct MAV_APM_S{
     float parm1;
     float parm2;
     float parm3;
@@ -124,7 +131,26 @@
     uint8_t targSys;
     uint8_t targComp;
     uint8_t confirm;
-} mav_APM;
+} MAV_APM;
+
+typedef struct MAV_MISSION_ITEM_S{
+    float parm1;
+    float parm2;
+    float parm3;
+    float parm4;
+    float lat;
+    float lon;
+    float alt;
+    uint16_t seq; 
+    uint16_t cmd; 
+    uint8_t targSys;
+    uint8_t targComp;
+    uint8_t frame;
+    uint8_t current; // set to true/1 to make current
+    uint8_t autoContinue;// set to true/1 to auto continue
+    uint8_t confirm;
+} MAV_MISSION_ITEM;
+
 
 int main()
 {
@@ -162,30 +188,59 @@
     USB::getSerial().printf("System startup...\n");
     char nextCmd[512+1];
     int readIndex=0;
-    bool reading=false;
     
     MAV_REQUEST_LIST req;req.targSys=1;req.targComp=1;
-    mav_APM issueCmd;
-    issueCmd.targSys=1;issueCmd.targComp=1;issueCmd.cmd=17;issueCmd.parm1=76;issueCmd.parm2=5.0f;issueCmd.parm2=6.0f;issueCmd.parm3=7.0f;
+    MAV_APM issueCmd;issueCmd.targSys=1;issueCmd.targComp=1;issueCmd.cmd=17;issueCmd.parm1=76;issueCmd.parm2=5.0f;issueCmd.parm2=6.0f;issueCmd.parm3=7.0f;
+    MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
+    MAV_MISSION_ITEM issueItem;issueItem.targSys=1;issueItem.targComp=1;issueItem.lat=5.0f;issueItem.lon=6.0f;issueItem.alt=7.0f;issueItem.cmd=16;issueItem.seq=0;issueItem.current=1;
+    bool firstRun=true;
+    bool gotNewRead=false;
+    int readState=0;
+    int realLen=0;
     while(true){
+        gotNewRead=false;
         if(Mav::getSerial().readable()>0){
             char input=Mav::getSerial().getc();
-            USB::getSerial().printf("> %x\n",input);
-            if(input==0xFE){
-                reading=true;
+            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
+            if(readState==0&&input==0xFE){
+                readState=1;
                 readIndex=1;
-            }else if(reading){
+            }else if(readState!=0){
                 nextCmd[std::min(readIndex++,512)]=input;
+                if(readState==1){
+                    realLen=input+5-1;
+                    readState=2;
+                }
+                if(readState==2){
+                    realLen--;
+                    if(realLen==0){
+                        readState=0;
+                        gotNewRead=true;
+                    }
+                }
             }
         }
         // Output debug info
-        if(readIndex==1&&reading){
+        if(gotNewRead){
             USB::getSerial().printf("Got CMD len %d messageid %d \n",nextCmd[1],nextCmd[5]);
             if(nextCmd[5]==0){
                 //Mav::sendOutput(0,input,9);
-                //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,(char*)&issueCmd,sizeof(mav_APM));
-                Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
+                //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,(char*)&issueCmd,sizeof(MAV_APM));
                 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
+                if(firstRun){
+                    USB::getSerial().printf("Issue Command\n",nextCmd[1],nextCmd[5]);
+                    Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+                    //Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
+                    //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
+                }
+                firstRun=false;
+            }
+            if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
+                USB::getSerial().printf("Issue Item\n");
+                MAV_REQ* incnt=(MAV_REQ*)(&(nextCmd[6]));
+                issueItem.seq=nextCmd[6];//incnt->count;
+                USB::getSerial().printf("Got request for %d %d\n",nextCmd[9],incnt->count);
+                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
             }
         }
     }