Revision 66:5d43988d100c, committed 2014-05-05

Comitter:

dylanembed123

Date:

Mon May 05 13:20:35 2014 +0000

Parent:

65:8e491bde6629

Commit message:

Final Project;

Changed in this revision

--- a/handle/dataLocation.h	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/dataLocation.h	Mon May 05 13:20:35 2014 +0000
@@ -30,11 +30,12 @@
    DataLoc loc;
 public:
     DataLocation(){}
-    DataLocation(double latA,double lonA,double altA,double timestampA=0){
+    DataLocation(double latA,double lonA,double altA,double timestampA=0,double headingA=0){
         loc.lat=latA;
         loc.lon=lonA;
         loc.alt=altA;
         loc.timestamp=timestampA;
+        loc.heading=headingA;
     }
     double& getLat(){return loc.lat;}
     double& getLon(){return loc.lon;}

--- a/handle/handleCamera.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/handleCamera.cpp	Mon May 05 13:20:35 2014 +0000
@@ -59,9 +59,9 @@
     wait_us(1000000);
     
     outputDevice.printf("Image End\n",size);
-    DH::Locs().getC().getAlt()=5;
-    DH::Locs().getC().getHeading()=5;
-    DH::Locs().getC().getTilt()=5;
+    //DH::Locs().getC().getAlt()=5;
+    //DH::Locs().getC().getHeading()=5;
+    //DH::Locs().getC().getTilt()=5;
     getPS().openConnection();
     getPS().sendPacket(0,NULL,0,PT_EMPTY);
     getPS().sendPacket(sID,NULL,0,PT_IMAGEHEAD);

--- a/handle/handleCompass.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/handleCompass.cpp	Mon May 05 13:20:35 2014 +0000
@@ -19,5 +19,5 @@
     }
     compass.read();
     heading = compass.get_heading();
-    //USB::getSerial().printf("Compass heading: %f\r\n",heading);
+    USB::getSerial().printf("Compass heading: %f\r\n",heading);
 }
\ No newline at end of file

--- a/handle/handleGPS.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/handleGPS.cpp	Mon May 05 13:20:35 2014 +0000
@@ -28,7 +28,15 @@
     unsigned int num_points = pack.size;
     for(int i=0;i<num_points;i++){
         //USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon);
-        DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,6.0f));
+        if(i==num_points-1){
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,75.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,25.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,10.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,5.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,0.0f));
+        }else{
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,150.0f));
+        }
     }
     //USB::getSerial().printf("Waypoints size: %d\r\n",DH::Locs().getI(LHType_targ,LHIType_size));
     for(int i=0;i<DH::Locs().getI(LHType_targ,LHIType_size);i++){
@@ -52,7 +60,9 @@
     unsigned int sID=getPS().getSuperID();
     getPS().sendPacket(0,NULL,0,PT_EMPTY);
     getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
+    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
     getPS().sendPacket(sID,(char*)(&DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs))),sizeof(DataLocation));
+    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
     getPS().sendPacket(sID,NULL,0,PT_END);
     wait_us(100000);
     getPS().closeConnection();
@@ -60,8 +70,8 @@
 }
 
 bool GPSHandle::if_image_location(){
-    double lon_thresh = 0.0005;
-    double lat_thresh = 0.00035;
+    double lon_thresh = 0.00030;
+    double lat_thresh = 0.00018;
     //USB::getSerial().printf("Checking if at waypoint\r\n");
     DataLocation current_loc = DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs));
     DataLocation next_waypoint = DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ));

--- a/handle/mavcommands.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/mavcommands.cpp	Mon May 05 13:20:35 2014 +0000
@@ -10,8 +10,8 @@
 static void fixLatLonAlt(double& latitude,double& longitude,double& altitude){
     latitude/=10000000.0f;
     longitude/=10000000.0f;
-    altitude/=10000000.0f;
-    USB::getSerial().printf("Got2 lat %f\n",latitude);USB::getSerial().printf("Got lon2 %f\n",longitude);USB::getSerial().printf("Got alt2 %f\n",altitude);
+    //altitude/=10000000.0f;
+    //USB::getSerial().printf("Got2 lat %f\n",latitude);USB::getSerial().printf("Got lon2 %f\n",longitude);USB::getSerial().printf("Got alt2 %f\n",altitude);
     return;
     int degrees;
     double minutes;
@@ -30,7 +30,7 @@
     USB::getSerial().printf("HM Lat %f\n",nLat);
     USB::getSerial().printf("HM Lon %f\n",nLon);
     USB::getSerial().printf("HM Alt %f\n",nAlt);
-    if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
+    //if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
     moveValid=false;
     cLat=nLat;
     cLon=nLon;
@@ -46,7 +46,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=1;
+    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=5;
     // 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
@@ -116,6 +116,7 @@
         }
         // Check for mavlink message request
         if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
+            hasMoved();
             USB::getSerial().printf("Issue Item %f %f %f\n",cLat,cLon,cAlt);
             // Set sequence to requested sequence
             issueItem.seq=myCmd[6];
@@ -149,11 +150,19 @@
             //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);
-            double lat=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);double alt=(double)*((int32_t*)&myCmd[10+8]);
+            //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=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);
+            double alt=(double)*((int32_t*)&myCmd[10-4]);
             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));
+            if(calibAlt==0){
+                calibAlt=alt;
+            }
+            USB::getSerial().printf("Got lat %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt-calibAlt);USB::getSerial().printf("Got aalt %f\n",alt);
+            USB::getSerial().printf("Got heading %f\n",compassHandle::getCompassHand().heading);
+            DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt-calibAlt,0,compassHandle::getCompassHand().heading));
+            USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
+            // Issue count to init waypoint entry
+            Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
         }
         // Check for waypoint count
         if(myCmd[5]==44){

--- a/handle/mavcommands.h	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/mavcommands.h	Mon May 05 13:20:35 2014 +0000
@@ -4,6 +4,8 @@
 #include "packet.h"
 #include <algorithm>
 #include"handle/dataLocation.h"
+#include"handle/handleCompass.h"
+
 typedef struct MAV_REQUEST_LIST_S{
     char targSys;
     char targComp;
@@ -96,17 +98,19 @@
     double cLat,cLon,cAlt;
     bool hasMoved();
     bool moveValid;
+    double calibAlt;
 public:
     MavCmd():initialized(false){}
     char* getNextCmd();
     void handleNextCmd();
     void setupCmds();
-    void setup(){if(!initialized){setupCmds();initialized=true;}}
+    void setup(){if(!initialized){setupCmds();initialized=true;calibAlt=0.0f;}}
     void run(){
         setup();
         readState=0;
         while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();}
-        if(cLon!=0&&cLat!=0&&hasMoved()){
+        //&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLat()!=0&&DH::Locs().getC(LHType_locs,DH::Locs().getI()).getLon()!=0
+        if(1==2&&DH::Locs().getI(LHType_targ,LHIType_size)>0&&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));

--- a/main.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/main.cpp	Mon May 05 13:20:35 2014 +0000
@@ -119,6 +119,7 @@
     //while(true){
     //    USB::getSerial().printf(".\n",DELAYBOOT);
     //    MavCmd::get().run();
+    //    compassHandle::getCompassHand().run();
     //    wait(5);
     //}