Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: handle/handleGPS.cpp
- Revision:
- 66:5d43988d100c
- Parent:
- 64:d4818fb7813c
--- 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));