Revision 52:b4dddb28dffa, committed 2014-04-26

Comitter:

dylanembed123

Date:

Sat Apr 26 03:35:53 2014 +0000

Parent:

40:7b4d6043f533

Child:

53:d785a6f6abdd

Commit message:

Need to pull;

Changed in this revision

--- a/handle/mavcommands.cpp	Wed Apr 23 04:09:06 2014 +0000
+++ b/handle/mavcommands.cpp	Sat Apr 26 03:35:53 2014 +0000
@@ -20,7 +20,9 @@
     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;
     // Issue start
     issueStart.targSys=1;issueStart.targComp=1;issueStart.lat=5.0f;issueStart.lon=6.0f;issueStart.alt=7.0f;issueStart.cmd=22;issueStart.seq=0;issueStart.current=1;
-
+    // Issue location request
+    issueStreamReq.targSys=1;issueStreamReq.targComp=1;issueStreamReq.streamID=6;issueStreamReq.rate=5;issueStreamReq.start=1;
+    
     // Local variables
     startSetup=true;// Set to true to initiate startup sequence
     readState=0;     // Read State
@@ -35,7 +37,7 @@
             return NULL;
         }else{
             char input=Mav::getSerial().getc();
-            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
+            USB::getSerial().printf(">%x ",input);
             if(readState==0&&input==0xFE){
                 readState=1;
                 readIndex=1;
@@ -48,6 +50,7 @@
                 if(readState==2){
                     realLen--;
                     if(realLen==0){
+                        USB::getSerial().printf("\n");
                         readState=0;
                         char* output=new char[readIndex];
                         for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
@@ -70,10 +73,14 @@
                 startSetup=false;
                 USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
                 // Issue count to init waypoint entry
-                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
-            }else{
-                Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
-            }
+                //Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+                // Start sending location data
+                //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+            }//else{
+            // Start sending location data
+            Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+                //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
+            //}
         }
         // Check for mavlink message request
         if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
@@ -92,9 +99,20 @@
             // Take off
             Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
             wait(1);
+            // Start sending location data
+            Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+            wait(1);
             // Arm motors
             Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
         }
+        // Check for GPS
+        if(myCmd[5]==33){
+            for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);}
+            MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6];
+            USB::getSerial().printf("Got lat %d\n",input->lat);// %d %d %d %d %d,input->lon,input->alt,input->x,input->y,input->z);
+            USB::getSerial().printf("Got lon %d\n",input->lon);
+            USB::getSerial().printf("Got alt %d\n",input->alt);
+        }
         // Check for waypoint count
         if(myCmd[5]==44){
             USB::getSerial().printf("Waypoints tsys %d tcomp %d cnt %d\n",myCmd[8],myCmd[7],myCmd[6]);

--- a/handle/mavcommands.h	Wed Apr 23 04:09:06 2014 +0000
+++ b/handle/mavcommands.h	Sat Apr 26 03:35:53 2014 +0000
@@ -54,13 +54,22 @@
 } MAV_MISSION_ITEM;
 
 typedef struct MAV_DATA_STREAM_S{
+    uint16_t rate; // Hz
     uint8_t targSys;
     uint8_t targComp;
     uint8_t streamID;
-    uint16_t rate; // Hz
     uint8_t start; // Set to 1 to start and 0 to stop
 } MAV_DATA_STREAM;
 
+typedef struct MAV_LOCDATA_S{
+    uint32_t lat;
+    uint32_t lon;
+    uint32_t alt;
+    uint16_t x;
+    uint16_t y;
+    uint16_t z;
+}MAV_LOCDATA;
+
 class MavCmd{
 private:
     MAV_REQUEST_LIST req;
@@ -70,6 +79,7 @@
     MAV_MISSION_ITEM issueItem;
     MAV_MISSION_ITEM issueStart;
     MAV_MISSION_ITEM issueTakeOff;
+    MAV_DATA_STREAM issueStreamReq;
     
     // Local variables
     bool startSetup;    // Set to true to initiate startup sequence

--- a/handle/mavcontrol.cpp	Wed Apr 23 04:09:06 2014 +0000
+++ b/handle/mavcontrol.cpp	Sat Apr 26 03:35:53 2014 +0000
@@ -64,7 +64,8 @@
     char* output=generatePacket(messageID,payload,length,&outLength);
     for(int i=0;i<outLength;i++){
         getSerial().putc(output[i]);
-        USB::getSerial().printf("+ %x\n",output[i]);
+        USB::getSerial().printf("+%x ",output[i]);
     }
+    USB::getSerial().printf("\n");
     delete output;
 }
\ No newline at end of file

--- a/main.cpp	Wed Apr 23 04:09:06 2014 +0000
+++ b/main.cpp	Sat Apr 26 03:35:53 2014 +0000
@@ -112,13 +112,13 @@
 {
     
     // Start Mav test
-    /*
-    USB::getSerial().printf("Wait 20\n");
+    
+    USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
     wait(DELAYBOOT);    
     while(true){
         MavCmd::get().run();
     }
-    */
+    
     // End mav test
     
     //handlers