Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

Files at this revision

API Documentation at this revision

Revision 61:aa32e17f6801, committed 2014-04-27

Comitter:
dylanembed123
Date:
Sun Apr 27 02:31:00 2014 +0000
Parent:
60:bf851bafc807
Child:
62:31ed14d02627
Child:
63:6e4afaa03d07
Commit message:
Did some strange trickery to get the flight controller to actually return lat, lon and alt before it apparently crashes? I'm starting to see why they didn't include a location function...

Changed in this revision

handle/mavcommands.cpp Show annotated file Show diff for this revision Revisions of this file
handle/mavcommands.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/handle/mavcommands.cpp	Sat Apr 26 23:01:45 2014 +0000
+++ b/handle/mavcommands.cpp	Sun Apr 27 02:31:00 2014 +0000
@@ -43,7 +43,7 @@
     // Issue disarm motors
     issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
     // Issue a mission count
-    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=5;
+    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
     // Issue a mission item
     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=0;issueItem.frame=0;issueItem.confirm=0;
     // Issue a take off item
@@ -62,18 +62,17 @@
 }
 
 char* MavCmd::getNextCmd(){
-    for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
-        if(Mav::getSerial().readable()<=0){
-            //wait_ms(10);
-            // Nothing to run, wait until next time
-            continue;//return NULL;
-        }else{
+        if(Mav::getSerial().readable()>0){
             char input=Mav::getSerial().getc();
-            USB::getSerial().printf(">%x ",input);
+            bool start=false;
             if(readState==0&&input==0xFE){
+                USB::getSerial().printf("\nS: ");
                 readState=1;
                 readIndex=1;
-            }else if(readState!=0){
+                start=true;
+            }
+            USB::getSerial().printf(">%x~",input);
+            if(!start&&readState!=0){
                 nextCmd[std::min(readIndex++,512)]=input;
                 if(readState==1){
                     realLen=input+5-1;
@@ -81,8 +80,9 @@
                 }
                 if(readState==2){
                     realLen--;
+                    if(readIndex-1==5&&nextCmd[5]==33){realLen=12;}
                     if(realLen==0){
-                        USB::getSerial().printf("\n");
+                        USB::getSerial().printf(" E\n");
                         readState=0;
                         char* output=new char[readIndex];
                         for(int i=0;i<readIndex;i++){output[i]=nextCmd[i];}
@@ -91,7 +91,7 @@
                 }
             }
         }
-    }
+        return NULL;
 }
 
 // Handle the next command (if one is available)
@@ -101,20 +101,11 @@
     if(myCmd!=NULL){
         USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
         if(myCmd[5]==0){
-            USB::getSerial().printf("Issue Command\n",myCmd[1],myCmd[5]);
+            //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));
             // Start sending location data
             //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
-            if(hasMoved()){
-                USB::getSerial().printf("USING NEW WAYPOINT!!!\n",myCmd[1],myCmd[5]);
-                // Issue count to init waypoint entry
-                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
-                wait_ms(1);
-            }else{
-                Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
-                wait_ms(1);
-            }
             //else{
             // Start sending location data
                 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
@@ -151,7 +142,7 @@
         }
         // 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]);}
+            //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);USB::getSerial().printf("Got lon %d\n",input->lon);USB::getSerial().printf("Got alt %d\n",input->alt);

--- a/handle/mavcommands.h	Sat Apr 26 23:01:45 2014 +0000
+++ b/handle/mavcommands.h	Sun Apr 27 02:31:00 2014 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "mavcontrol.h"
 #include "adapt/usb.h"
+#include "packet.h"
 #include <algorithm>
 #include"handle/dataLocation.h"
 typedef struct MAV_REQUEST_LIST_S{
@@ -62,13 +63,15 @@
 } MAV_DATA_STREAM;
 
 typedef struct MAV_LOCDATA_S{
-    int32_t timestamp;
+    int32_t alt;//timestamp;
     int32_t lat;
     int32_t lon;
-    int32_t alt;
-    uint16_t x;
-    uint16_t y;
-    uint16_t z;
+    int32_t altB;
+    //int32_t ralt;
+    //int16_t x;
+    //int16_t y;
+    //int16_t z;
+    //uint16_t hdg;
 }MAV_LOCDATA;
 
 class MavCmd{
@@ -101,7 +104,27 @@
     void setup(){if(!initialized){setupCmds();initialized=true;}}
     void run(){
         setup();
-        for(int i=0;i<100;i++){readState=0;handleNextCmd();wait_ms(1);}
+        readState=0;
+        while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();}
+        if(hasMoved()){
+            USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
+            // Issue count to init waypoint entry
+            Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+        }else{
+            USB::getSerial().printf("Grabbing Stream\n");
+            Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+        }
+        //wait_ms(10);
+        for(int i=0;i<1000;i++){
+            if(getPS().rx_ready_with_timeout(&Mav::getSerial(),0,1000)==1){
+            }else{
+                readState=0;
+            }
+            handleNextCmd();
+            wait_ms(1);
+            USB::getSerial().printf(".");
+        }
+        USB::getSerial().printf("\n");
     }
     static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
 };

--- a/main.cpp	Sat Apr 26 23:01:45 2014 +0000
+++ b/main.cpp	Sun Apr 27 02:31:00 2014 +0000
@@ -113,13 +113,14 @@
 {
     
     // Start Mav test
-    //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
+    DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
     USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
     wait(DELAYBOOT);    
-    //while(true){
-//        MavCmd::get().run();
-//        wait(1);
-//    }
+    while(true){
+        USB::getSerial().printf(".\n",DELAYBOOT);
+        MavCmd::get().run();
+        wait(5);
+    }
 
     USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
     USB::getSerial().printf("Check GPS\n");