Colin Stearns / Mbed 2 deprecated 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.

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/handleGPS.cpp@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
64:d4818fb7813c 
Final Project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dylanembed123 9:da906eeac51e 1 #include "handleGPS.h"
stearnsc 8:28b866df62cf 2
dylanembed123 9:da906eeac51e 3 //Serial gps(p28,p27);
stearnsc 8:28b866df62cf 4
krobertson 20:81d5655fecc2 5 GPSHandle* GPSHandle::hand = NULL;
krobertson 20:81d5655fecc2 6
dylanembed123 14:6be57da62283 7 void testVar(){
dylanembed123 14:6be57da62283 8 }
dylanembed123 7:c75d5e5e6bfc 9 void GPSHandle::setup(){
dylanembed123 14:6be57da62283 10 //gpsGPS().getSerial().baud(57600);
stearnsc 8:28b866df62cf 11 sendGpsCommand("PMTK301,1");
dylanembed123 14:6be57da62283 12 //GPS::getSerial().attach(&GPSHandle::handleUpdate,Serial::RxIrq);
krobertson 32:9cb7bc3fc9e0 13 //GPS::getSerial().attach(this,&GPSHandle::handleUpdate,Serial::RxIrq);
stearnsc 8:28b866df62cf 14 //cs: Send other standard init commands? Not strictly speaking necessary,
stearnsc 8:28b866df62cf 15 // but forces "up to date documentation" in the form of always knowing
stearnsc 8:28b866df62cf 16 // gps config.
dylanembed123 7:c75d5e5e6bfc 17 }
dylanembed123 7:c75d5e5e6bfc 18
krobertson 20:81d5655fecc2 19 char GPSHandle::readWaypoints(){
krobertson 51:d6b64ac3c30d 20 //USB::getSerial().printf("getting waypoitns\r\n");
krobertson 20:81d5655fecc2 21 PacketStruct pack;
krobertson 20:81d5655fecc2 22 char rx_status = getPS().receivePacket(&pack);
krobertson 20:81d5655fecc2 23 if(rx_status != 1){
krobertson 20:81d5655fecc2 24 return rx_status;
krobertson 20:81d5655fecc2 25 }
krobertson 44:2ae602b89961 26 DH::Locs().getI(LHType_targ,LHIType_size) = 0;//reset size to 0 (clear out old waypoints
krobertson 20:81d5655fecc2 27 Point* points = (Point*)pack.data;
krobertson 20:81d5655fecc2 28 unsigned int num_points = pack.size;
krobertson 20:81d5655fecc2 29 for(int i=0;i<num_points;i++){
krobertson 51:d6b64ac3c30d 30 //USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon);
dylanembed123 66:5d43988d100c 31 if(i==num_points-1){
dylanembed123 66:5d43988d100c 32 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,75.0f));
dylanembed123 66:5d43988d100c 33 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,25.0f));
dylanembed123 66:5d43988d100c 34 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,10.0f));
dylanembed123 66:5d43988d100c 35 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,5.0f));
dylanembed123 66:5d43988d100c 36 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,0.0f));
dylanembed123 66:5d43988d100c 37 }else{
dylanembed123 66:5d43988d100c 38 DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,150.0f));
dylanembed123 66:5d43988d100c 39 }
krobertson 20:81d5655fecc2 40 }
krobertson 51:d6b64ac3c30d 41 //USB::getSerial().printf("Waypoints size: %d\r\n",DH::Locs().getI(LHType_targ,LHIType_size));
dylanembed123 34:c0b13ce5408c 42 for(int i=0;i<DH::Locs().getI(LHType_targ,LHIType_size);i++){
dylanembed123 34:c0b13ce5408c 43 DataLocation thisData=DH::Locs().getC(LHType_targ,i);
krobertson 32:9cb7bc3fc9e0 44 USB::getSerial().printf("Waypoint %d: %f,%f\r\n",i,thisData.getLat(),thisData.getLon());
krobertson 32:9cb7bc3fc9e0 45 }
krobertson 32:9cb7bc3fc9e0 46 DH::Locs().inc(LHType_targ,0,true);
krobertson 20:81d5655fecc2 47 return 1;
krobertson 20:81d5655fecc2 48 }
krobertson 20:81d5655fecc2 49
krobertson 32:9cb7bc3fc9e0 50 void GPSHandle::next_waypoint(){
krobertson 32:9cb7bc3fc9e0 51 DH::Locs().inc(LHType_targ);
krobertson 32:9cb7bc3fc9e0 52 USB::getSerial().printf("Sending Quadcopter to next waypoint\r\n");
krobertson 32:9cb7bc3fc9e0 53 //code to command quad to go to next waypoint goes here
krobertson 32:9cb7bc3fc9e0 54 }
krobertson 32:9cb7bc3fc9e0 55
krobertson 20:81d5655fecc2 56 void GPSHandle::sendLoc(){
krobertson 20:81d5655fecc2 57 wait_us(100000);
krobertson 20:81d5655fecc2 58 getPS().openConnection();
krobertson 20:81d5655fecc2 59 wait_us(100000);
krobertson 20:81d5655fecc2 60 unsigned int sID=getPS().getSuperID();
krobertson 20:81d5655fecc2 61 getPS().sendPacket(0,NULL,0,PT_EMPTY);
krobertson 20:81d5655fecc2 62 getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
dylanembed123 66:5d43988d100c 63 DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
krobertson 32:9cb7bc3fc9e0 64 getPS().sendPacket(sID,(char*)(&DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs))),sizeof(DataLocation));
dylanembed123 66:5d43988d100c 65 DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
krobertson 20:81d5655fecc2 66 getPS().sendPacket(sID,NULL,0,PT_END);
krobertson 20:81d5655fecc2 67 wait_us(100000);
krobertson 20:81d5655fecc2 68 getPS().closeConnection();
krobertson 20:81d5655fecc2 69 wait_us(100000);
krobertson 20:81d5655fecc2 70 }
krobertson 20:81d5655fecc2 71
krobertson 20:81d5655fecc2 72 bool GPSHandle::if_image_location(){
dylanembed123 66:5d43988d100c 73 double lon_thresh = 0.00030;
dylanembed123 66:5d43988d100c 74 double lat_thresh = 0.00018;
krobertson 49:06721139d298 75 //USB::getSerial().printf("Checking if at waypoint\r\n");
krobertson 32:9cb7bc3fc9e0 76 DataLocation current_loc = DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs));
krobertson 32:9cb7bc3fc9e0 77 DataLocation next_waypoint = DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ));
krobertson 32:9cb7bc3fc9e0 78 USB::getSerial().printf("current: %f,%f ... waypoint: %f,%f \r\n",current_loc.getLat(),current_loc.getLon(),next_waypoint.getLat(),next_waypoint.getLon());
krobertson 32:9cb7bc3fc9e0 79 double lat_diff = (current_loc.getLat()>next_waypoint.getLat()) ? current_loc.getLat() - next_waypoint.getLat() : next_waypoint.getLat() - current_loc.getLat();
krobertson 32:9cb7bc3fc9e0 80 double lon_diff = (current_loc.getLon()>next_waypoint.getLon()) ? current_loc.getLon() - next_waypoint.getLon() : next_waypoint.getLon() - current_loc.getLon();
krobertson 32:9cb7bc3fc9e0 81 USB::getSerial().printf("lat diff: %f, lon dif %f \r\n",lat_diff,lon_diff);
krobertson 32:9cb7bc3fc9e0 82 return (lat_diff < lat_thresh && lon_diff < lon_thresh);
krobertson 20:81d5655fecc2 83 }
krobertson 20:81d5655fecc2 84
dylanembed123 14:6be57da62283 85 static bool reading = false;
dylanembed123 14:6be57da62283 86 //static std::stringstream line;
dylanembed123 14:6be57da62283 87 static char line[MAXREADIN+10];
dylanembed123 14:6be57da62283 88 static int line_i=0;
dylanembed123 14:6be57da62283 89 char* getNext(char*& field){
dylanembed123 14:6be57da62283 90 char* output=new char[MAXREADIN+1];
dylanembed123 14:6be57da62283 91 int i;
dylanembed123 14:6be57da62283 92 for(i=0;i<MAXREADIN;i++){
dylanembed123 14:6be57da62283 93 if(field[i]=='\0'||field[i]==',')break;
dylanembed123 14:6be57da62283 94 }
dylanembed123 14:6be57da62283 95 for(int a=0;a<i;a++){
dylanembed123 14:6be57da62283 96 output[a]=field[a];
dylanembed123 14:6be57da62283 97 }
dylanembed123 14:6be57da62283 98 output[i]='\0';
dylanembed123 14:6be57da62283 99 field=&field[i+1];
dylanembed123 14:6be57da62283 100 return output;
dylanembed123 14:6be57da62283 101 }
dylanembed123 14:6be57da62283 102
stearnsc 8:28b866df62cf 103 void GPSHandle::handleUpdate(){
krobertson 32:9cb7bc3fc9e0 104 char c;
krobertson 32:9cb7bc3fc9e0 105 reading = false;
krobertson 43:b8cbe6f0ec47 106 while(getPS().rx_ready_with_timeout(&GPS::getSerial(),0,10000)){
krobertson 32:9cb7bc3fc9e0 107 c = GPS::getSerial().getc();
krobertson 41:df156ae5631b 108 //USB::getSerial().printf("%c",c);
krobertson 36:53b69e471b5a 109 if (reading) {
krobertson 36:53b69e471b5a 110 if(line_i>=MAXREADIN){reading=false;return;}
krobertson 36:53b69e471b5a 111 if (c == '*') { //sentence buffer complete; we're ignoring the checksum
krobertson 36:53b69e471b5a 112 char* field=line;
krobertson 36:53b69e471b5a 113 char* op;
krobertson 36:53b69e471b5a 114 op=getNext(field);delete op; //GPGGA
krobertson 36:53b69e471b5a 115 if(op[0]=='G'||op[1]=='P'||op[2]=='G'||op[3]=='G'||op[4]=='A'){
krobertson 36:53b69e471b5a 116 op=getNext(field);double timeS = atof(op);delete op; //time
krobertson 36:53b69e471b5a 117 op=getNext(field);double latitude = atof(op);delete op; //latitude
krobertson 36:53b69e471b5a 118 op=getNext(field);delete op; //N or S
krobertson 36:53b69e471b5a 119 op=getNext(field);double longitude = atof(op);delete op; //longitude
krobertson 36:53b69e471b5a 120 op=getNext(field);delete op; //E or W
krobertson 36:53b69e471b5a 121 op=getNext(field);delete op; //skip
krobertson 36:53b69e471b5a 122 op=getNext(field);delete op; //skip
krobertson 36:53b69e471b5a 123 op=getNext(field);delete op; //skip
krobertson 36:53b69e471b5a 124 op=getNext(field);delete op; //altitude
krobertson 36:53b69e471b5a 125 double altitude = atof(op);
krobertson 36:53b69e471b5a 126 if(timeS>0.5f){
krobertson 38:6f814050895d 127 int degrees = (int)(latitude/100);
krobertson 38:6f814050895d 128 double minutes = latitude - degrees*100;
krobertson 38:6f814050895d 129 latitude = degrees + minutes/60;
krobertson 38:6f814050895d 130 degrees = (int)(longitude/100);
krobertson 38:6f814050895d 131 minutes = longitude - degrees*100;
krobertson 51:d6b64ac3c30d 132 longitude = -1*(degrees + minutes/60);
krobertson 36:53b69e471b5a 133 USB::getSerial().printf("\nMy GPS data: Lat: %f, Lon: %f, Alt: %f, Time:%f\r\n",latitude,longitude,altitude,timeS);
krobertson 36:53b69e471b5a 134 DH::Locs().add(LHType_locs,DataLocation(latitude,longitude,altitude,timeS));
krobertson 41:df156ae5631b 135 // USB::getSerial().printf("Current Time:%f\r\n",DH::Locs().getC().getTime());
krobertson 36:53b69e471b5a 136 return;
krobertson 36:53b69e471b5a 137 }
krobertson 36:53b69e471b5a 138 }
krobertson 36:53b69e471b5a 139 //update whatever needs updating when gps updates
krobertson 36:53b69e471b5a 140 //pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
krobertson 36:53b69e471b5a 141 //gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
krobertson 36:53b69e471b5a 142 //);
krobertson 36:53b69e471b5a 143 reading = false;
krobertson 36:53b69e471b5a 144 } else {
krobertson 36:53b69e471b5a 145 line[line_i]=c;
krobertson 36:53b69e471b5a 146 line_i=(line_i+1)%MAXREADIN;
krobertson 36:53b69e471b5a 147 }
krobertson 36:53b69e471b5a 148 }else if (c == '$') {
krobertson 32:9cb7bc3fc9e0 149 reading = true;
krobertson 32:9cb7bc3fc9e0 150 line_i=0;
krobertson 32:9cb7bc3fc9e0 151 }
krobertson 32:9cb7bc3fc9e0 152 }
dylanembed123 14:6be57da62283 153 return;
stearnsc 8:28b866df62cf 154 }
stearnsc 8:28b866df62cf 155
stearnsc 8:28b866df62cf 156 //sends: "$<command>*<checksum>\r\l"
dylanembed123 9:da906eeac51e 157 void GPSHandle::sendGpsCommand(std::string command){
stearnsc 8:28b866df62cf 158 uint8_t checksum = 0;
dylanembed123 9:da906eeac51e 159 //pc.printf("Sending command to gps: ");
dylanembed123 14:6be57da62283 160 GPS::getSerial().putc('$');
dylanembed123 9:da906eeac51e 161 //pc.putc('$');
stearnsc 8:28b866df62cf 162 char c;
stearnsc 8:28b866df62cf 163 for (int i = 0; i < command.length(); i++) {
stearnsc 8:28b866df62cf 164 c = command[i];
stearnsc 8:28b866df62cf 165 checksum ^= c;
dylanembed123 14:6be57da62283 166 GPS::getSerial().putc(c);
dylanembed123 9:da906eeac51e 167 //pc.putc(c);
stearnsc 8:28b866df62cf 168 }
dylanembed123 14:6be57da62283 169 GPS::getSerial().putc('*');
dylanembed123 9:da906eeac51e 170 //pc.putc('*');
stearnsc 8:28b866df62cf 171 string checkSumString;
stearnsc 8:28b866df62cf 172 while (checksum > 0) {
stearnsc 8:28b866df62cf 173 uint8_t checksumChar = checksum & 0x0F;
stearnsc 8:28b866df62cf 174 if (checksumChar >= 10) {
stearnsc 8:28b866df62cf 175 checksumChar -= 10;
stearnsc 8:28b866df62cf 176 checksumChar += 'A';
stearnsc 8:28b866df62cf 177 } else {
stearnsc 8:28b866df62cf 178 checksumChar += '0';
stearnsc 8:28b866df62cf 179 }
stearnsc 8:28b866df62cf 180 checkSumString.push_back((char) checksumChar);
stearnsc 8:28b866df62cf 181 checksum = checksum >> 4;
stearnsc 8:28b866df62cf 182 }
stearnsc 8:28b866df62cf 183
stearnsc 8:28b866df62cf 184 for (int i = checkSumString.length() - 1; i >= 0; i--) {
dylanembed123 14:6be57da62283 185 GPS::getSerial().putc(checkSumString[i]);
dylanembed123 9:da906eeac51e 186 //pc.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 187 }
dylanembed123 14:6be57da62283 188 GPS::getSerial().putc('\r');
dylanembed123 9:da906eeac51e 189 //pc.putc('\r');
dylanembed123 14:6be57da62283 190 GPS::getSerial().putc('\n');
dylanembed123 9:da906eeac51e 191 //pc.putc('\n');
stearnsc 8:28b866df62cf 192 }
stearnsc 8:28b866df62cf 193
stearnsc 8:28b866df62cf 194 int stringToDecimal(string s)
stearnsc 8:28b866df62cf 195 {
stearnsc 8:28b866df62cf 196 int mult = 1;
stearnsc 8:28b866df62cf 197 int result = 0;
stearnsc 8:28b866df62cf 198 for (int i = s.length() - 1; i >=0; i--) {
stearnsc 8:28b866df62cf 199 if (s[i] != '.') {
stearnsc 8:28b866df62cf 200 result += (s[i] - '0') * mult;
stearnsc 8:28b866df62cf 201 mult *= 10;
dylanembed123 7:c75d5e5e6bfc 202 }
dylanembed123 7:c75d5e5e6bfc 203 }
stearnsc 8:28b866df62cf 204 return result;
dylanembed123 7:c75d5e5e6bfc 205 }
dylanembed123 7:c75d5e5e6bfc 206
dylanembed123 7:c75d5e5e6bfc 207 bool GPSHandle::check(){
dylanembed123 7:c75d5e5e6bfc 208 return true;
dylanembed123 14:6be57da62283 209 }
dylanembed123 14:6be57da62283 210 void GPSHandle::run(){
dylanembed123 14:6be57da62283 211 if(!initialized){initialized=true;setup();}
krobertson 32:9cb7bc3fc9e0 212 handleUpdate();
dylanembed123 7:c75d5e5e6bfc 213 }