Diff: main.cpp

Revision:

23:497f8faa908e

Parent:

22:9880a26886db

Child:

24:e65416d6de22

--- a/main.cpp	Tue Apr 22 00:10:16 2014 +0000
+++ b/main.cpp	Tue Apr 22 13:29:31 2014 +0000
@@ -107,9 +107,9 @@
 }MAV_REQUEST_LIST;
 
 typedef struct MAV_COUNT_S{
+    uint16_t count;
     char targSys;
     char targComp;
-    uint16_t count; 
 }MAV_COUNT;
 
 typedef struct MAV_REQ_S{
@@ -154,6 +154,9 @@
 
 int main()
 {
+    USB::getSerial().printf("Wait 20\n");
+    wait(20);
+    /*
     char input[9];
     input[0]=0x00;
     input[1]=0x00;
@@ -190,9 +193,17 @@
     int readIndex=0;
     
     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;
+    // Issue arm motors
+    MAV_APM issueArm;issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
+    // Issue disarm motors
+    MAV_APM issueDisArm;issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
+    // Issue a mission count
     MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
+    // Issue a mission item
     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;
+    // Issue a take off item
+    MAV_MISSION_ITEM issueTakeOff;issueTakeOff.targSys=1;issueTakeOff.targComp=1;issueTakeOff.lat=5.0f;issueTakeOff.lon=6.0f;issueTakeOff.alt=7.0f;issueTakeOff.cmd=22;issueTakeOff.seq=0;issueTakeOff.current=1;
+    
     bool firstRun=true;
     bool gotNewRead=false;
     int readState=0;
@@ -225,7 +236,7 @@
             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_LONG,(char*)&issueArm,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]);
@@ -242,6 +253,12 @@
                 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));
             }
+            if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
+                // Arm motors
+                Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
+                // Take off
+                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
+            }
         }
     }