Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/handleGPS.cpp@31:6f68fa0aeee5, 2014-04-22 (annotated)

Committer:
krobertson
Date:
Tue Apr 22 15:40:23 2014 +0000
Revision:
31:6f68fa0aeee5
Parent:
30:327191ff57e8
Parent:
29:b47b4fd78c00
Child:
32:9cb7bc3fc9e0
adding if image location

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);
dylanembed123 14:6be57da62283 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 20:81d5655fecc2 26 Point* points = (Point*)pack.data;
krobertson 20:81d5655fecc2 27 unsigned int num_points = pack.size;
krobertson 20:81d5655fecc2 28 for(int i=0;i<num_points;i++){
krobertson 20:81d5655fecc2 29 USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon);
krobertson 20:81d5655fecc2 30 }
krobertson 20:81d5655fecc2 31 return 1;
krobertson 20:81d5655fecc2 32 }
krobertson 20:81d5655fecc2 33
krobertson 20:81d5655fecc2 34 void GPSHandle::sendLoc(){
krobertson 20:81d5655fecc2 35 wait_us(100000);
krobertson 20:81d5655fecc2 36 getPS().openConnection();
krobertson 20:81d5655fecc2 37 wait_us(100000);
krobertson 20:81d5655fecc2 38 unsigned int sID=getPS().getSuperID();
krobertson 20:81d5655fecc2 39 getPS().sendPacket(0,NULL,0,PT_EMPTY);
krobertson 20:81d5655fecc2 40 getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
krobertson 30:327191ff57e8 41 getPS().sendPacket(sID,(char*)(&DH::locs().getC(LHType_locs,DH::locs().getI(LHType_locs))),sizeof(DataLocation));
krobertson 20:81d5655fecc2 42 getPS().sendPacket(sID,NULL,0,PT_END);
krobertson 20:81d5655fecc2 43 wait_us(100000);
krobertson 20:81d5655fecc2 44 getPS().closeConnection();
krobertson 20:81d5655fecc2 45 wait_us(100000);
krobertson 20:81d5655fecc2 46 }
krobertson 20:81d5655fecc2 47
krobertson 20:81d5655fecc2 48 bool GPSHandle::if_image_location(){
krobertson 29:b47b4fd78c00 49 double lon_thresh = 0.00005;
krobertson 29:b47b4fd78c00 50 double lat_thresh = 0.000035;
krobertson 20:81d5655fecc2 51 USB::getSerial().printf("Checking if at waypoint\r\n");
krobertson 31:6f68fa0aeee5 52 DataLocation current_loc = DH::locs().getC(LHType_locs,DH::locs().getI(LHType_locs));
krobertson 29:b47b4fd78c00 53 //if(dl.lat
krobertson 20:81d5655fecc2 54 return true;
krobertson 20:81d5655fecc2 55 }
krobertson 20:81d5655fecc2 56
dylanembed123 14:6be57da62283 57 static bool reading = false;
dylanembed123 14:6be57da62283 58 //static std::stringstream line;
dylanembed123 14:6be57da62283 59 static char line[MAXREADIN+10];
dylanembed123 14:6be57da62283 60 static int line_i=0;
dylanembed123 14:6be57da62283 61 char* getNext(char*& field){
dylanembed123 14:6be57da62283 62 char* output=new char[MAXREADIN+1];
dylanembed123 14:6be57da62283 63 int i;
dylanembed123 14:6be57da62283 64 for(i=0;i<MAXREADIN;i++){
dylanembed123 14:6be57da62283 65 if(field[i]=='\0'||field[i]==',')break;
dylanembed123 14:6be57da62283 66 }
dylanembed123 14:6be57da62283 67 for(int a=0;a<i;a++){
dylanembed123 14:6be57da62283 68 output[a]=field[a];
dylanembed123 14:6be57da62283 69 }
dylanembed123 14:6be57da62283 70 output[i]='\0';
dylanembed123 14:6be57da62283 71 field=&field[i+1];
dylanembed123 14:6be57da62283 72 return output;
dylanembed123 14:6be57da62283 73 }
dylanembed123 14:6be57da62283 74
stearnsc 8:28b866df62cf 75 void GPSHandle::handleUpdate(){
dylanembed123 14:6be57da62283 76 if(GPS::getSerial().readable()<=0){return;}
dylanembed123 14:6be57da62283 77 char c = GPS::getSerial().getc();
dylanembed123 14:6be57da62283 78 //USB::getSerial().printf("%c",c);
stearnsc 8:28b866df62cf 79 if (reading) {
dylanembed123 14:6be57da62283 80 if(line_i>=MAXREADIN){reading=false;return;}
stearnsc 8:28b866df62cf 81 if (c == '*') { //sentence buffer complete; we're ignoring the checksum
dylanembed123 14:6be57da62283 82 char* field=line;
dylanembed123 14:6be57da62283 83 char* op;
dylanembed123 14:6be57da62283 84 op=getNext(field);delete op; //GPGGA
dylanembed123 14:6be57da62283 85 if(op[0]=='G'||op[1]=='P'||op[2]=='G'||op[3]=='G'||op[4]=='A'){
dylanembed123 14:6be57da62283 86 op=getNext(field);double timeS = atof(op);delete op; //time
dylanembed123 14:6be57da62283 87 op=getNext(field);double latitude = atof(op);delete op; //latitude
dylanembed123 14:6be57da62283 88 op=getNext(field);delete op; //N or S
dylanembed123 14:6be57da62283 89 op=getNext(field);double longitude = atof(op);delete op; //longitude
dylanembed123 14:6be57da62283 90 op=getNext(field);delete op; //E or W
dylanembed123 14:6be57da62283 91 op=getNext(field);delete op; //skip
dylanembed123 14:6be57da62283 92 op=getNext(field);delete op; //skip
dylanembed123 14:6be57da62283 93 op=getNext(field);delete op; //skip
dylanembed123 14:6be57da62283 94 op=getNext(field);delete op; //altitude
dylanembed123 14:6be57da62283 95 double altitude = atof(op);
dylanembed123 16:4f5d20b87dc3 96 if(timeS>0.5f){
dylanembed123 15:e3e03a9df89e 97 //USB::getSerial().printf("\nMy GPS data: Lat: %f, Lon: %f, Alt: %f, Time:%f\r\n",latitude,longitude,altitude,timeS);
dylanembed123 15:e3e03a9df89e 98 DH::Locs().add(LHType_locs,DataLocation(latitude,longitude,altitude,timeS));
dylanembed123 15:e3e03a9df89e 99 //USB::getSerial().printf("Current Time:%f\r\n",DH::Locs().getC().getTime());
dylanembed123 16:4f5d20b87dc3 100 }
dylanembed123 14:6be57da62283 101 }
stearnsc 8:28b866df62cf 102 //update whatever needs updating when gps updates
stearnsc 8:28b866df62cf 103 // pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 8:28b866df62cf 104 // gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
stearnsc 8:28b866df62cf 105 // );
stearnsc 8:28b866df62cf 106
stearnsc 8:28b866df62cf 107 reading = false;
stearnsc 8:28b866df62cf 108 } else {
dylanembed123 14:6be57da62283 109 line[line_i]=c;
dylanembed123 14:6be57da62283 110 line_i=(line_i+1)%MAXREADIN;
stearnsc 8:28b866df62cf 111 }
stearnsc 8:28b866df62cf 112
stearnsc 8:28b866df62cf 113 } else if (c == '$') {
stearnsc 8:28b866df62cf 114 reading = true;
dylanembed123 14:6be57da62283 115 line_i=0;
stearnsc 8:28b866df62cf 116 }
dylanembed123 14:6be57da62283 117 return;
stearnsc 8:28b866df62cf 118 }
stearnsc 8:28b866df62cf 119
stearnsc 8:28b866df62cf 120 //sends: "$<command>*<checksum>\r\l"
dylanembed123 9:da906eeac51e 121 void GPSHandle::sendGpsCommand(std::string command){
stearnsc 8:28b866df62cf 122 uint8_t checksum = 0;
dylanembed123 9:da906eeac51e 123 //pc.printf("Sending command to gps: ");
dylanembed123 14:6be57da62283 124 GPS::getSerial().putc('$');
dylanembed123 9:da906eeac51e 125 //pc.putc('$');
stearnsc 8:28b866df62cf 126 char c;
stearnsc 8:28b866df62cf 127 for (int i = 0; i < command.length(); i++) {
stearnsc 8:28b866df62cf 128 c = command[i];
stearnsc 8:28b866df62cf 129 checksum ^= c;
dylanembed123 14:6be57da62283 130 GPS::getSerial().putc(c);
dylanembed123 9:da906eeac51e 131 //pc.putc(c);
stearnsc 8:28b866df62cf 132 }
dylanembed123 14:6be57da62283 133 GPS::getSerial().putc('*');
dylanembed123 9:da906eeac51e 134 //pc.putc('*');
stearnsc 8:28b866df62cf 135 string checkSumString;
stearnsc 8:28b866df62cf 136 while (checksum > 0) {
stearnsc 8:28b866df62cf 137 uint8_t checksumChar = checksum & 0x0F;
stearnsc 8:28b866df62cf 138 if (checksumChar >= 10) {
stearnsc 8:28b866df62cf 139 checksumChar -= 10;
stearnsc 8:28b866df62cf 140 checksumChar += 'A';
stearnsc 8:28b866df62cf 141 } else {
stearnsc 8:28b866df62cf 142 checksumChar += '0';
stearnsc 8:28b866df62cf 143 }
stearnsc 8:28b866df62cf 144 checkSumString.push_back((char) checksumChar);
stearnsc 8:28b866df62cf 145 checksum = checksum >> 4;
stearnsc 8:28b866df62cf 146 }
stearnsc 8:28b866df62cf 147
stearnsc 8:28b866df62cf 148 for (int i = checkSumString.length() - 1; i >= 0; i--) {
dylanembed123 14:6be57da62283 149 GPS::getSerial().putc(checkSumString[i]);
dylanembed123 9:da906eeac51e 150 //pc.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 151 }
dylanembed123 14:6be57da62283 152 GPS::getSerial().putc('\r');
dylanembed123 9:da906eeac51e 153 //pc.putc('\r');
dylanembed123 14:6be57da62283 154 GPS::getSerial().putc('\n');
dylanembed123 9:da906eeac51e 155 //pc.putc('\n');
stearnsc 8:28b866df62cf 156 }
stearnsc 8:28b866df62cf 157
stearnsc 8:28b866df62cf 158 int stringToDecimal(string s)
stearnsc 8:28b866df62cf 159 {
stearnsc 8:28b866df62cf 160 int mult = 1;
stearnsc 8:28b866df62cf 161 int result = 0;
stearnsc 8:28b866df62cf 162 for (int i = s.length() - 1; i >=0; i--) {
stearnsc 8:28b866df62cf 163 if (s[i] != '.') {
stearnsc 8:28b866df62cf 164 result += (s[i] - '0') * mult;
stearnsc 8:28b866df62cf 165 mult *= 10;
dylanembed123 7:c75d5e5e6bfc 166 }
dylanembed123 7:c75d5e5e6bfc 167 }
stearnsc 8:28b866df62cf 168 return result;
dylanembed123 7:c75d5e5e6bfc 169 }
dylanembed123 7:c75d5e5e6bfc 170
dylanembed123 7:c75d5e5e6bfc 171 bool GPSHandle::check(){
dylanembed123 7:c75d5e5e6bfc 172 return true;
dylanembed123 14:6be57da62283 173 }
dylanembed123 14:6be57da62283 174 void GPSHandle::run(){
dylanembed123 14:6be57da62283 175 if(!initialized){initialized=true;setup();}
dylanembed123 7:c75d5e5e6bfc 176 }