Colin Stearns
/
qcControl
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
dgps
by 
Colin Stearns
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);
//}