Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
handle/handleGPS.cpp@49:06721139d298, 2014-04-26 (annotated)
- Committer:
- krobertson
- Date:
- Sat Apr 26 04:14:18 2014 +0000
- Revision:
- 49:06721139d298
- Parent:
- 44:2ae602b89961
- Child:
- 51:d6b64ac3c30d
minor changes involving output for debugging. I need to pull and this software screws up when I do
Who changed what in which revision?
User | Revision | Line number | New 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 | 20:81d5655fecc2 | 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 | 20:81d5655fecc2 | 30 | USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon); |
krobertson | 32:9cb7bc3fc9e0 | 31 | DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,6.0f)); |
krobertson | 20:81d5655fecc2 | 32 | } |
dylanembed123 | 34:c0b13ce5408c | 33 | USB::getSerial().printf("Waypoints size: %d\r\n",DH::Locs().getI(LHType_targ,LHIType_size)); |
dylanembed123 | 34:c0b13ce5408c | 34 | for(int i=0;i<DH::Locs().getI(LHType_targ,LHIType_size);i++){ |
dylanembed123 | 34:c0b13ce5408c | 35 | DataLocation thisData=DH::Locs().getC(LHType_targ,i); |
krobertson | 32:9cb7bc3fc9e0 | 36 | USB::getSerial().printf("Waypoint %d: %f,%f\r\n",i,thisData.getLat(),thisData.getLon()); |
krobertson | 32:9cb7bc3fc9e0 | 37 | } |
krobertson | 32:9cb7bc3fc9e0 | 38 | DH::Locs().inc(LHType_targ,0,true); |
krobertson | 20:81d5655fecc2 | 39 | return 1; |
krobertson | 20:81d5655fecc2 | 40 | } |
krobertson | 20:81d5655fecc2 | 41 | |
krobertson | 32:9cb7bc3fc9e0 | 42 | void GPSHandle::next_waypoint(){ |
krobertson | 32:9cb7bc3fc9e0 | 43 | DH::Locs().inc(LHType_targ); |
krobertson | 32:9cb7bc3fc9e0 | 44 | USB::getSerial().printf("Sending Quadcopter to next waypoint\r\n"); |
krobertson | 32:9cb7bc3fc9e0 | 45 | //code to command quad to go to next waypoint goes here |
krobertson | 32:9cb7bc3fc9e0 | 46 | } |
krobertson | 32:9cb7bc3fc9e0 | 47 | |
krobertson | 20:81d5655fecc2 | 48 | void GPSHandle::sendLoc(){ |
krobertson | 20:81d5655fecc2 | 49 | wait_us(100000); |
krobertson | 20:81d5655fecc2 | 50 | getPS().openConnection(); |
krobertson | 20:81d5655fecc2 | 51 | wait_us(100000); |
krobertson | 20:81d5655fecc2 | 52 | unsigned int sID=getPS().getSuperID(); |
krobertson | 20:81d5655fecc2 | 53 | getPS().sendPacket(0,NULL,0,PT_EMPTY); |
krobertson | 20:81d5655fecc2 | 54 | getPS().sendPacket(sID,NULL,0,PT_SENDLOC); |
krobertson | 32:9cb7bc3fc9e0 | 55 | getPS().sendPacket(sID,(char*)(&DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs))),sizeof(DataLocation)); |
krobertson | 20:81d5655fecc2 | 56 | getPS().sendPacket(sID,NULL,0,PT_END); |
krobertson | 20:81d5655fecc2 | 57 | wait_us(100000); |
krobertson | 20:81d5655fecc2 | 58 | getPS().closeConnection(); |
krobertson | 20:81d5655fecc2 | 59 | wait_us(100000); |
krobertson | 20:81d5655fecc2 | 60 | } |
krobertson | 20:81d5655fecc2 | 61 | |
krobertson | 20:81d5655fecc2 | 62 | bool GPSHandle::if_image_location(){ |
krobertson | 29:b47b4fd78c00 | 63 | double lon_thresh = 0.00005; |
krobertson | 29:b47b4fd78c00 | 64 | double lat_thresh = 0.000035; |
krobertson | 49:06721139d298 | 65 | //USB::getSerial().printf("Checking if at waypoint\r\n"); |
krobertson | 32:9cb7bc3fc9e0 | 66 | DataLocation current_loc = DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)); |
krobertson | 32:9cb7bc3fc9e0 | 67 | DataLocation next_waypoint = DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ)); |
krobertson | 32:9cb7bc3fc9e0 | 68 | 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 | 69 | double lat_diff = (current_loc.getLat()>next_waypoint.getLat()) ? current_loc.getLat() - next_waypoint.getLat() : next_waypoint.getLat() - current_loc.getLat(); |
krobertson | 32:9cb7bc3fc9e0 | 70 | double lon_diff = (current_loc.getLon()>next_waypoint.getLon()) ? current_loc.getLon() - next_waypoint.getLon() : next_waypoint.getLon() - current_loc.getLon(); |
krobertson | 32:9cb7bc3fc9e0 | 71 | USB::getSerial().printf("lat diff: %f, lon dif %f \r\n",lat_diff,lon_diff); |
krobertson | 32:9cb7bc3fc9e0 | 72 | return (lat_diff < lat_thresh && lon_diff < lon_thresh); |
krobertson | 20:81d5655fecc2 | 73 | } |
krobertson | 20:81d5655fecc2 | 74 | |
dylanembed123 | 14:6be57da62283 | 75 | static bool reading = false; |
dylanembed123 | 14:6be57da62283 | 76 | //static std::stringstream line; |
dylanembed123 | 14:6be57da62283 | 77 | static char line[MAXREADIN+10]; |
dylanembed123 | 14:6be57da62283 | 78 | static int line_i=0; |
dylanembed123 | 14:6be57da62283 | 79 | char* getNext(char*& field){ |
dylanembed123 | 14:6be57da62283 | 80 | char* output=new char[MAXREADIN+1]; |
dylanembed123 | 14:6be57da62283 | 81 | int i; |
dylanembed123 | 14:6be57da62283 | 82 | for(i=0;i<MAXREADIN;i++){ |
dylanembed123 | 14:6be57da62283 | 83 | if(field[i]=='\0'||field[i]==',')break; |
dylanembed123 | 14:6be57da62283 | 84 | } |
dylanembed123 | 14:6be57da62283 | 85 | for(int a=0;a<i;a++){ |
dylanembed123 | 14:6be57da62283 | 86 | output[a]=field[a]; |
dylanembed123 | 14:6be57da62283 | 87 | } |
dylanembed123 | 14:6be57da62283 | 88 | output[i]='\0'; |
dylanembed123 | 14:6be57da62283 | 89 | field=&field[i+1]; |
dylanembed123 | 14:6be57da62283 | 90 | return output; |
dylanembed123 | 14:6be57da62283 | 91 | } |
dylanembed123 | 14:6be57da62283 | 92 | |
stearnsc | 8:28b866df62cf | 93 | void GPSHandle::handleUpdate(){ |
krobertson | 32:9cb7bc3fc9e0 | 94 | char c; |
krobertson | 32:9cb7bc3fc9e0 | 95 | reading = false; |
krobertson | 43:b8cbe6f0ec47 | 96 | while(getPS().rx_ready_with_timeout(&GPS::getSerial(),0,10000)){ |
krobertson | 32:9cb7bc3fc9e0 | 97 | c = GPS::getSerial().getc(); |
krobertson | 41:df156ae5631b | 98 | //USB::getSerial().printf("%c",c); |
krobertson | 36:53b69e471b5a | 99 | if (reading) { |
krobertson | 36:53b69e471b5a | 100 | if(line_i>=MAXREADIN){reading=false;return;} |
krobertson | 36:53b69e471b5a | 101 | if (c == '*') { //sentence buffer complete; we're ignoring the checksum |
krobertson | 36:53b69e471b5a | 102 | char* field=line; |
krobertson | 36:53b69e471b5a | 103 | char* op; |
krobertson | 36:53b69e471b5a | 104 | op=getNext(field);delete op; //GPGGA |
krobertson | 36:53b69e471b5a | 105 | if(op[0]=='G'||op[1]=='P'||op[2]=='G'||op[3]=='G'||op[4]=='A'){ |
krobertson | 36:53b69e471b5a | 106 | op=getNext(field);double timeS = atof(op);delete op; //time |
krobertson | 36:53b69e471b5a | 107 | op=getNext(field);double latitude = atof(op);delete op; //latitude |
krobertson | 36:53b69e471b5a | 108 | op=getNext(field);delete op; //N or S |
krobertson | 36:53b69e471b5a | 109 | op=getNext(field);double longitude = atof(op);delete op; //longitude |
krobertson | 36:53b69e471b5a | 110 | op=getNext(field);delete op; //E or W |
krobertson | 36:53b69e471b5a | 111 | op=getNext(field);delete op; //skip |
krobertson | 36:53b69e471b5a | 112 | op=getNext(field);delete op; //skip |
krobertson | 36:53b69e471b5a | 113 | op=getNext(field);delete op; //skip |
krobertson | 36:53b69e471b5a | 114 | op=getNext(field);delete op; //altitude |
krobertson | 36:53b69e471b5a | 115 | double altitude = atof(op); |
krobertson | 36:53b69e471b5a | 116 | if(timeS>0.5f){ |
krobertson | 38:6f814050895d | 117 | int degrees = (int)(latitude/100); |
krobertson | 38:6f814050895d | 118 | double minutes = latitude - degrees*100; |
krobertson | 38:6f814050895d | 119 | latitude = degrees + minutes/60; |
krobertson | 38:6f814050895d | 120 | degrees = (int)(longitude/100); |
krobertson | 38:6f814050895d | 121 | minutes = longitude - degrees*100; |
krobertson | 38:6f814050895d | 122 | longitude = degrees + minutes/60; |
krobertson | 36:53b69e471b5a | 123 | USB::getSerial().printf("\nMy GPS data: Lat: %f, Lon: %f, Alt: %f, Time:%f\r\n",latitude,longitude,altitude,timeS); |
krobertson | 36:53b69e471b5a | 124 | DH::Locs().add(LHType_locs,DataLocation(latitude,longitude,altitude,timeS)); |
krobertson | 41:df156ae5631b | 125 | // USB::getSerial().printf("Current Time:%f\r\n",DH::Locs().getC().getTime()); |
krobertson | 36:53b69e471b5a | 126 | return; |
krobertson | 36:53b69e471b5a | 127 | } |
krobertson | 36:53b69e471b5a | 128 | } |
krobertson | 36:53b69e471b5a | 129 | //update whatever needs updating when gps updates |
krobertson | 36:53b69e471b5a | 130 | //pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", |
krobertson | 36:53b69e471b5a | 131 | //gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time |
krobertson | 36:53b69e471b5a | 132 | //); |
krobertson | 36:53b69e471b5a | 133 | reading = false; |
krobertson | 36:53b69e471b5a | 134 | } else { |
krobertson | 36:53b69e471b5a | 135 | line[line_i]=c; |
krobertson | 36:53b69e471b5a | 136 | line_i=(line_i+1)%MAXREADIN; |
krobertson | 36:53b69e471b5a | 137 | } |
krobertson | 36:53b69e471b5a | 138 | }else if (c == '$') { |
krobertson | 32:9cb7bc3fc9e0 | 139 | reading = true; |
krobertson | 32:9cb7bc3fc9e0 | 140 | line_i=0; |
krobertson | 32:9cb7bc3fc9e0 | 141 | } |
krobertson | 32:9cb7bc3fc9e0 | 142 | } |
dylanembed123 | 14:6be57da62283 | 143 | return; |
stearnsc | 8:28b866df62cf | 144 | } |
stearnsc | 8:28b866df62cf | 145 | |
stearnsc | 8:28b866df62cf | 146 | //sends: "$<command>*<checksum>\r\l" |
dylanembed123 | 9:da906eeac51e | 147 | void GPSHandle::sendGpsCommand(std::string command){ |
stearnsc | 8:28b866df62cf | 148 | uint8_t checksum = 0; |
dylanembed123 | 9:da906eeac51e | 149 | //pc.printf("Sending command to gps: "); |
dylanembed123 | 14:6be57da62283 | 150 | GPS::getSerial().putc('$'); |
dylanembed123 | 9:da906eeac51e | 151 | //pc.putc('$'); |
stearnsc | 8:28b866df62cf | 152 | char c; |
stearnsc | 8:28b866df62cf | 153 | for (int i = 0; i < command.length(); i++) { |
stearnsc | 8:28b866df62cf | 154 | c = command[i]; |
stearnsc | 8:28b866df62cf | 155 | checksum ^= c; |
dylanembed123 | 14:6be57da62283 | 156 | GPS::getSerial().putc(c); |
dylanembed123 | 9:da906eeac51e | 157 | //pc.putc(c); |
stearnsc | 8:28b866df62cf | 158 | } |
dylanembed123 | 14:6be57da62283 | 159 | GPS::getSerial().putc('*'); |
dylanembed123 | 9:da906eeac51e | 160 | //pc.putc('*'); |
stearnsc | 8:28b866df62cf | 161 | string checkSumString; |
stearnsc | 8:28b866df62cf | 162 | while (checksum > 0) { |
stearnsc | 8:28b866df62cf | 163 | uint8_t checksumChar = checksum & 0x0F; |
stearnsc | 8:28b866df62cf | 164 | if (checksumChar >= 10) { |
stearnsc | 8:28b866df62cf | 165 | checksumChar -= 10; |
stearnsc | 8:28b866df62cf | 166 | checksumChar += 'A'; |
stearnsc | 8:28b866df62cf | 167 | } else { |
stearnsc | 8:28b866df62cf | 168 | checksumChar += '0'; |
stearnsc | 8:28b866df62cf | 169 | } |
stearnsc | 8:28b866df62cf | 170 | checkSumString.push_back((char) checksumChar); |
stearnsc | 8:28b866df62cf | 171 | checksum = checksum >> 4; |
stearnsc | 8:28b866df62cf | 172 | } |
stearnsc | 8:28b866df62cf | 173 | |
stearnsc | 8:28b866df62cf | 174 | for (int i = checkSumString.length() - 1; i >= 0; i--) { |
dylanembed123 | 14:6be57da62283 | 175 | GPS::getSerial().putc(checkSumString[i]); |
dylanembed123 | 9:da906eeac51e | 176 | //pc.putc(checkSumString[i]); |
stearnsc | 8:28b866df62cf | 177 | } |
dylanembed123 | 14:6be57da62283 | 178 | GPS::getSerial().putc('\r'); |
dylanembed123 | 9:da906eeac51e | 179 | //pc.putc('\r'); |
dylanembed123 | 14:6be57da62283 | 180 | GPS::getSerial().putc('\n'); |
dylanembed123 | 9:da906eeac51e | 181 | //pc.putc('\n'); |
stearnsc | 8:28b866df62cf | 182 | } |
stearnsc | 8:28b866df62cf | 183 | |
stearnsc | 8:28b866df62cf | 184 | int stringToDecimal(string s) |
stearnsc | 8:28b866df62cf | 185 | { |
stearnsc | 8:28b866df62cf | 186 | int mult = 1; |
stearnsc | 8:28b866df62cf | 187 | int result = 0; |
stearnsc | 8:28b866df62cf | 188 | for (int i = s.length() - 1; i >=0; i--) { |
stearnsc | 8:28b866df62cf | 189 | if (s[i] != '.') { |
stearnsc | 8:28b866df62cf | 190 | result += (s[i] - '0') * mult; |
stearnsc | 8:28b866df62cf | 191 | mult *= 10; |
dylanembed123 | 7:c75d5e5e6bfc | 192 | } |
dylanembed123 | 7:c75d5e5e6bfc | 193 | } |
stearnsc | 8:28b866df62cf | 194 | return result; |
dylanembed123 | 7:c75d5e5e6bfc | 195 | } |
dylanembed123 | 7:c75d5e5e6bfc | 196 | |
dylanembed123 | 7:c75d5e5e6bfc | 197 | bool GPSHandle::check(){ |
dylanembed123 | 7:c75d5e5e6bfc | 198 | return true; |
dylanembed123 | 14:6be57da62283 | 199 | } |
dylanembed123 | 14:6be57da62283 | 200 | void GPSHandle::run(){ |
dylanembed123 | 14:6be57da62283 | 201 | if(!initialized){initialized=true;setup();} |
krobertson | 32:9cb7bc3fc9e0 | 202 | handleUpdate(); |
dylanembed123 | 7:c75d5e5e6bfc | 203 | } |