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 54:fc7c8b5d4d41, committed 2014-04-26

Comitter:
dylanembed123
Date:
Sat Apr 26 16:26:18 2014 +0000
Parent:
53:d785a6f6abdd
Child:
55:c2c10a521874
Commit message:
Update mutliple send attempts of path.

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 04:09:11 2014 +0000
+++ b/handle/mavcommands.cpp	Sat Apr 26 16:26:18 2014 +0000
@@ -3,9 +3,13 @@
 // Maximium number of charictors to read without giving up the processor
 #define MAX_READ_LOOP 256
 
+#define ENABLE_MOTORS false
+
 MavCmd* MavCmd::mavcmd=NULL;
 
-static void fixLatLon(double& latitude,double& longitude){
+static void fixLatLonAlt(double& latitude,double& longitude,double& altitude){
+    latitude/=10000000.0f;
+    longitude/=10000000.0f;
     int degrees;
     double minutes;
     degrees = (int)(latitude/100);
@@ -16,6 +20,18 @@
     longitude = degrees + minutes/60;    
 }
 
+bool MavCmd::hasMoved(){
+    double nLat=DH::Locs().getC(LHType_targ).getLat();
+    double nLon=DH::Locs().getC(LHType_targ).getLon();
+    double nAlt=DH::Locs().getC(LHType_targ).getAlt();
+    if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
+    moveValid=false;
+    cLat=nLat;
+    cLon=nLon;
+    cAlt=nAlt;
+    return true;
+}
+
 void MavCmd::setupCmds(){
     // Request item list
     req.targSys=1;req.targComp=1;
@@ -39,6 +55,7 @@
     readState=0;     // Read State
     realLen=0;       // How many more bytes need to be read
     readIndex=0;     // Current index in next cmd (also the size)
+    cLat=cLon=cAlt=0.0f;
 }
 
 char* MavCmd::getNextCmd(){
@@ -80,49 +97,61 @@
     if(myCmd!=NULL){
         USB::getSerial().printf("Got CMD len %d messageid %d \n",myCmd[1],myCmd[5]);
         if(myCmd[5]==0){
-            if(startSetup){
-                startSetup=false;
-                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));
-                // Start sending location data
-                //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
-            }//else{
+                Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+            }else{
+                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){
-            USB::getSerial().printf("Issue Item\n");
+            USB::getSerial().printf("Issue Item %f %f %f\n",cLat,cLon,cAlt);
             // Set sequence to requested sequence
             issueItem.seq=myCmd[6];
-            issueItem.lon=(float)issueItem.seq;
+            issueItem.lat=(float)cLat;
+            issueItem.lon=(float)cLon;
+            issueItem.alt=(float)cAlt;
             // Issue mission item
             Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
         }
         // Check for mission accepted
         if(myCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
-            // Start
-            Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM));
-            wait(1);
-            // 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));
+            moveValid=true;
+            if(startSetup&&ENABLE_MOTORS){
+                startSetup=false;
+                // Start
+                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueStart,sizeof(MAV_MISSION_ITEM));
+                wait(1);
+                // 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);
+            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);
+            double lat=input->lat;double lon=input->lon;double alt=input->alt;
+            fixLatLonAlt(lat,lon,alt);
+            USB::getSerial().printf("Got lat fixed %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt);
+            DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt));
         }
         // Check for waypoint count
         if(myCmd[5]==44){

--- a/handle/mavcommands.h	Sat Apr 26 04:09:11 2014 +0000
+++ b/handle/mavcommands.h	Sat Apr 26 16:26:18 2014 +0000
@@ -2,7 +2,7 @@
 #include "mavcontrol.h"
 #include "adapt/usb.h"
 #include <algorithm>
-
+#include"handle/dataLocation.h"
 typedef struct MAV_REQUEST_LIST_S{
     char targSys;
     char targComp;
@@ -62,9 +62,9 @@
 } MAV_DATA_STREAM;
 
 typedef struct MAV_LOCDATA_S{
+    uint32_t alt;
     uint32_t lat;
     uint32_t lon;
-    uint32_t alt;
     uint16_t x;
     uint16_t y;
     uint16_t z;
@@ -89,6 +89,9 @@
     int readIndex;       // Current index in next cmd (also the size)
     static MavCmd* mavcmd;
     bool initialized;
+    double cLat,cLon,cAlt;
+    bool hasMoved();
+    bool moveValid;
 public:
     MavCmd():initialized(false){}
     char* getNextCmd();

--- a/main.cpp	Sat Apr 26 04:09:11 2014 +0000
+++ b/main.cpp	Sat Apr 26 16:26:18 2014 +0000
@@ -113,7 +113,7 @@
 {
     
     // Start Mav test
-    
+    DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
     USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
     wait(DELAYBOOT);    
     while(true){