Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
main.cpp@60:bf851bafc807, 2014-04-26 (annotated)
- Committer:
- krobertson
- Date:
- Sat Apr 26 23:01:45 2014 +0000
- Revision:
- 60:bf851bafc807
- Parent:
- 59:972b4eb0df94
- Child:
- 61:aa32e17f6801
gps working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stearnsc | 0:9c001c4e7bf4 | 1 | #include "mbed.h" |
stearnsc | 0:9c001c4e7bf4 | 2 | #include <string> |
stearnsc | 0:9c001c4e7bf4 | 3 | #include <sstream> |
dylanembed123 | 7:c75d5e5e6bfc | 4 | #include "adapt/usb.h" |
dylanembed123 | 9:da906eeac51e | 5 | #include "handle/handleCamera.h" |
dylanembed123 | 9:da906eeac51e | 6 | #include "handle/handleGPS.h" |
krobertson | 20:81d5655fecc2 | 7 | #include "handle/handleCommand.h" |
dylanembed123 | 26:06f1c9d70e9f | 8 | #include "handle/mavcommands.h" |
dylanembed123 | 33:ad63e7013801 | 9 | #include "handle/dataLocation.h" |
krobertson | 44:2ae602b89961 | 10 | #include "handle/handleCompass.h" |
dylanembed123 | 33:ad63e7013801 | 11 | #define DELAYBOOT 1 |
stearnsc | 8:28b866df62cf | 12 | |
stearnsc | 0:9c001c4e7bf4 | 13 | Serial pc(USBTX,USBRX); |
stearnsc | 0:9c001c4e7bf4 | 14 | Serial xbee(p9,p10);//tx, rx |
stearnsc | 0:9c001c4e7bf4 | 15 | Serial gps(p28,p27); |
dylanembed123 | 6:434d20e99e49 | 16 | Serial camera(p13,p14); |
stearnsc | 0:9c001c4e7bf4 | 17 | |
stearnsc | 0:9c001c4e7bf4 | 18 | typedef struct { |
stearnsc | 0:9c001c4e7bf4 | 19 | int latitude; //in .0001 minutes |
stearnsc | 0:9c001c4e7bf4 | 20 | int longitude; //in .0001 minutes |
stearnsc | 0:9c001c4e7bf4 | 21 | int altitude; //in decimeters |
stearnsc | 0:9c001c4e7bf4 | 22 | int time; //in milliseconds |
stearnsc | 0:9c001c4e7bf4 | 23 | } GpsData; |
stearnsc | 0:9c001c4e7bf4 | 24 | |
stearnsc | 8:28b866df62cf | 25 | void readSerial(Serial &s, char str[], int size) |
stearnsc | 8:28b866df62cf | 26 | { |
stearnsc | 8:28b866df62cf | 27 | for (int i = 0; i < size; i++) { |
stearnsc | 8:28b866df62cf | 28 | str[i] = s.getc(); |
stearnsc | 0:9c001c4e7bf4 | 29 | } |
stearnsc | 0:9c001c4e7bf4 | 30 | } |
stearnsc | 0:9c001c4e7bf4 | 31 | |
krobertson | 18:e72ee7aed088 | 32 | void connection_lost(){ |
krobertson | 18:e72ee7aed088 | 33 | USB::getSerial().printf("TCP connection lost!\r\n"); |
krobertson | 18:e72ee7aed088 | 34 | } |
krobertson | 18:e72ee7aed088 | 35 | |
stearnsc | 0:9c001c4e7bf4 | 36 | //sends: "$<command>*<checksum>\r\l" |
stearnsc | 8:28b866df62cf | 37 | void sendGpsCommand(string command) |
stearnsc | 8:28b866df62cf | 38 | { |
stearnsc | 0:9c001c4e7bf4 | 39 | uint8_t checksum = 0; |
stearnsc | 0:9c001c4e7bf4 | 40 | pc.printf("Sending command to gps: "); |
stearnsc | 0:9c001c4e7bf4 | 41 | gps.putc('$'); |
stearnsc | 0:9c001c4e7bf4 | 42 | pc.putc('$'); |
stearnsc | 0:9c001c4e7bf4 | 43 | char c; |
stearnsc | 8:28b866df62cf | 44 | for (int i = 0; i < command.length(); i++) { |
stearnsc | 0:9c001c4e7bf4 | 45 | c = command[i]; |
stearnsc | 0:9c001c4e7bf4 | 46 | checksum ^= c; |
stearnsc | 0:9c001c4e7bf4 | 47 | gps.putc(c); |
stearnsc | 0:9c001c4e7bf4 | 48 | pc.putc(c); |
stearnsc | 0:9c001c4e7bf4 | 49 | } |
stearnsc | 0:9c001c4e7bf4 | 50 | gps.putc('*'); |
stearnsc | 0:9c001c4e7bf4 | 51 | pc.putc('*'); |
stearnsc | 0:9c001c4e7bf4 | 52 | string checkSumString; |
stearnsc | 8:28b866df62cf | 53 | while (checksum > 0) { |
stearnsc | 0:9c001c4e7bf4 | 54 | uint8_t checksumChar = checksum & 0x0F; |
stearnsc | 8:28b866df62cf | 55 | if (checksumChar >= 10) { |
stearnsc | 0:9c001c4e7bf4 | 56 | checksumChar -= 10; |
stearnsc | 8:28b866df62cf | 57 | checksumChar += 'A'; |
stearnsc | 0:9c001c4e7bf4 | 58 | } else { |
stearnsc | 8:28b866df62cf | 59 | checksumChar += '0'; |
stearnsc | 0:9c001c4e7bf4 | 60 | } |
stearnsc | 0:9c001c4e7bf4 | 61 | checkSumString.push_back((char) checksumChar); |
stearnsc | 8:28b866df62cf | 62 | checksum = checksum >> 4; |
stearnsc | 0:9c001c4e7bf4 | 63 | } |
stearnsc | 0:9c001c4e7bf4 | 64 | |
stearnsc | 8:28b866df62cf | 65 | for (int i = checkSumString.length() - 1; i >= 0; i--) { |
stearnsc | 0:9c001c4e7bf4 | 66 | gps.putc(checkSumString[i]); |
stearnsc | 8:28b866df62cf | 67 | pc.putc(checkSumString[i]); |
stearnsc | 8:28b866df62cf | 68 | } |
stearnsc | 0:9c001c4e7bf4 | 69 | gps.putc('\r'); |
stearnsc | 0:9c001c4e7bf4 | 70 | pc.putc('\r'); |
stearnsc | 0:9c001c4e7bf4 | 71 | gps.putc('\n'); |
stearnsc | 0:9c001c4e7bf4 | 72 | pc.putc('\n'); |
stearnsc | 0:9c001c4e7bf4 | 73 | } |
stearnsc | 8:28b866df62cf | 74 | // |
stearnsc | 8:28b866df62cf | 75 | ////cs: little endian parsing |
stearnsc | 8:28b866df62cf | 76 | //int nextInt(char *data, int i) |
stearnsc | 8:28b866df62cf | 77 | //{ |
stearnsc | 8:28b866df62cf | 78 | // i |= data[i]; |
stearnsc | 8:28b866df62cf | 79 | // i |= (data[i + 1] << 8); |
stearnsc | 8:28b866df62cf | 80 | // i |= (data[i + 2] << 16); |
stearnsc | 8:28b866df62cf | 81 | // i |= (data[i + 3] << 24); |
stearnsc | 8:28b866df62cf | 82 | // return i; |
stearnsc | 8:28b866df62cf | 83 | //} |
stearnsc | 0:9c001c4e7bf4 | 84 | |
stearnsc | 8:28b866df62cf | 85 | //void handleXbeeGps() |
stearnsc | 8:28b866df62cf | 86 | //{ |
stearnsc | 8:28b866df62cf | 87 | // static bool reading = false; |
stearnsc | 8:28b866df62cf | 88 | // static char packet[16]; |
stearnsc | 8:28b866df62cf | 89 | // static int i = 0; |
stearnsc | 8:28b866df62cf | 90 | // |
stearnsc | 8:28b866df62cf | 91 | // char c = xbee.getc(); |
stearnsc | 8:28b866df62cf | 92 | // if (reading) { |
stearnsc | 8:28b866df62cf | 93 | // packet[i] = c; |
stearnsc | 8:28b866df62cf | 94 | // i++; |
stearnsc | 8:28b866df62cf | 95 | // if (i == 16) { |
stearnsc | 8:28b866df62cf | 96 | // i = 0; |
stearnsc | 8:28b866df62cf | 97 | // otherGps.latitude = nextInt(packet, 0); |
stearnsc | 8:28b866df62cf | 98 | // otherGps.longitude = nextInt(packet, 4); |
stearnsc | 8:28b866df62cf | 99 | // otherGps.altitude = nextInt(packet, 8); |
stearnsc | 8:28b866df62cf | 100 | // otherGps.time = nextInt(packet, 12); |
stearnsc | 8:28b866df62cf | 101 | // |
stearnsc | 8:28b866df62cf | 102 | // pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", |
stearnsc | 8:28b866df62cf | 103 | // otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time |
stearnsc | 8:28b866df62cf | 104 | // ); |
stearnsc | 8:28b866df62cf | 105 | // reading = false; |
stearnsc | 8:28b866df62cf | 106 | // } |
stearnsc | 8:28b866df62cf | 107 | // } else if (c == 'X') { |
stearnsc | 8:28b866df62cf | 108 | // reading = true; |
stearnsc | 8:28b866df62cf | 109 | // } |
stearnsc | 8:28b866df62cf | 110 | //} |
dylanembed123 | 7:c75d5e5e6bfc | 111 | |
stearnsc | 8:28b866df62cf | 112 | int main() |
stearnsc | 8:28b866df62cf | 113 | { |
dylanembed123 | 40:7b4d6043f533 | 114 | |
dylanembed123 | 24:e65416d6de22 | 115 | // Start Mav test |
krobertson | 58:ea73523cf04b | 116 | //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0)); |
dylanembed123 | 52:b4dddb28dffa | 117 | USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT); |
dylanembed123 | 33:ad63e7013801 | 118 | wait(DELAYBOOT); |
krobertson | 60:bf851bafc807 | 119 | //while(true){ |
krobertson | 60:bf851bafc807 | 120 | // MavCmd::get().run(); |
krobertson | 60:bf851bafc807 | 121 | // wait(1); |
krobertson | 60:bf851bafc807 | 122 | // } |
krobertson | 20:81d5655fecc2 | 123 | |
dylanembed123 | 13:a6d3cf2b018e | 124 | USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct)); |
dylanembed123 | 14:6be57da62283 | 125 | USB::getSerial().printf("Check GPS\n"); |
krobertson | 20:81d5655fecc2 | 126 | USB::getSerial().printf("Connect to the wifly network now!\r\n"); |
krobertson | 20:81d5655fecc2 | 127 | //XBEE::getTCPInterrupt().fall(&connection_lost); |
krobertson | 20:81d5655fecc2 | 128 | //checking connection to egg before continuing |
krobertson | 19:8c1f2a2204fb | 129 | getPS().openConnection(); |
krobertson | 19:8c1f2a2204fb | 130 | getPS().closeConnection(); |
krobertson | 20:81d5655fecc2 | 131 | //Main Loop |
krobertson | 41:df156ae5631b | 132 | int count = 0; |
krobertson | 19:8c1f2a2204fb | 133 | while(1){ |
krobertson | 49:06721139d298 | 134 | //USB::getSerial().printf("Running GPS...\r\n"); |
krobertson | 58:ea73523cf04b | 135 | //GPSHandle::getGPSHand().run(); |
krobertson | 58:ea73523cf04b | 136 | MavCmd::get().run(); |
krobertson | 49:06721139d298 | 137 | //USB::getSerial().printf("Running Compass...\r\n"); |
krobertson | 44:2ae602b89961 | 138 | compassHandle::getCompassHand().run(); |
krobertson | 58:ea73523cf04b | 139 | if(count % 10 == 0){ |
krobertson | 49:06721139d298 | 140 | USB::getSerial().printf("Requesting commands from egg...\r\n"); |
krobertson | 41:df156ae5631b | 141 | wait_us(100000); |
krobertson | 41:df156ae5631b | 142 | CommandHandle::getCommandHand().run(); |
krobertson | 41:df156ae5631b | 143 | wait_us(100000); |
krobertson | 58:ea73523cf04b | 144 | } |
krobertson | 58:ea73523cf04b | 145 | if(GPSHandle::getGPSHand().if_image_location()){ |
krobertson | 58:ea73523cf04b | 146 | USB::getSerial().printf("Taking picture and sending...\r\n"); |
krobertson | 58:ea73523cf04b | 147 | wait_us(100000); |
krobertson | 58:ea73523cf04b | 148 | ImageHandle::getImageHand().run(); |
krobertson | 58:ea73523cf04b | 149 | USB::getSerial().printf("sent all data\r\n"); |
krobertson | 58:ea73523cf04b | 150 | wait_us(100000); |
krobertson | 58:ea73523cf04b | 151 | GPSHandle::getGPSHand().next_waypoint(); |
krobertson | 58:ea73523cf04b | 152 | wait_us(100000); |
krobertson | 58:ea73523cf04b | 153 | }else{ |
krobertson | 58:ea73523cf04b | 154 | //USB::getSerial().printf("Not close enough to waypoint for image\r\n"); |
dylanembed123 | 21:c546eab07e28 | 155 | } |
krobertson | 41:df156ae5631b | 156 | count++; |
dylanembed123 | 11:97625c27ab90 | 157 | } |
dylanembed123 | 9:da906eeac51e | 158 | } |