Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@6:434d20e99e49, 2014-02-21 (annotated)

Committer:
dylanembed123
Date:
Fri Feb 21 15:15:25 2014 +0000
Revision:
6:434d20e99e49
Parent:
0:9c001c4e7bf4
Child:
7:c75d5e5e6bfc
Camera takes an image and sends it back to the USB.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stearnsc 0:9c001c4e7bf4 1 #include "mbed.h"
stearnsc 0:9c001c4e7bf4 2 #include <string>
stearnsc 0:9c001c4e7bf4 3 #include <sstream>
dylanembed123 6:434d20e99e49 4 #include "usb.h"
dylanembed123 6:434d20e99e49 5 #include "camera.h"
dylanembed123 6:434d20e99e49 6 /*
stearnsc 0:9c001c4e7bf4 7 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 8 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 9 Serial gps(p28,p27);
dylanembed123 6:434d20e99e49 10 Serial camera(p13,p14);
stearnsc 0:9c001c4e7bf4 11
stearnsc 0:9c001c4e7bf4 12 typedef struct {
stearnsc 0:9c001c4e7bf4 13 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 14 int longitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 15 int altitude; //in decimeters
stearnsc 0:9c001c4e7bf4 16 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 17 } GpsData;
stearnsc 0:9c001c4e7bf4 18
stearnsc 0:9c001c4e7bf4 19 GpsData gpsData;
stearnsc 0:9c001c4e7bf4 20 GpsData otherGps;
stearnsc 0:9c001c4e7bf4 21
stearnsc 0:9c001c4e7bf4 22 void readSerial(Serial &s, char str[], int size){
stearnsc 0:9c001c4e7bf4 23 for (int i = 0; i < size; i++){
stearnsc 0:9c001c4e7bf4 24 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 25 }
stearnsc 0:9c001c4e7bf4 26 }
stearnsc 0:9c001c4e7bf4 27
stearnsc 0:9c001c4e7bf4 28 //sends: "$<command>*<checksum>\r\l"
stearnsc 0:9c001c4e7bf4 29 void sendGpsCommand(string command){
stearnsc 0:9c001c4e7bf4 30 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 31 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 32 gps.putc('$');
stearnsc 0:9c001c4e7bf4 33 pc.putc('$');
stearnsc 0:9c001c4e7bf4 34 char c;
stearnsc 0:9c001c4e7bf4 35 for (int i = 0; i < command.length(); i++){
stearnsc 0:9c001c4e7bf4 36 c = command[i];
stearnsc 0:9c001c4e7bf4 37 checksum ^= c;
stearnsc 0:9c001c4e7bf4 38 gps.putc(c);
stearnsc 0:9c001c4e7bf4 39 pc.putc(c);
stearnsc 0:9c001c4e7bf4 40 }
stearnsc 0:9c001c4e7bf4 41 gps.putc('*');
stearnsc 0:9c001c4e7bf4 42 pc.putc('*');
stearnsc 0:9c001c4e7bf4 43 string checkSumString;
stearnsc 0:9c001c4e7bf4 44 while (checksum > 0){
stearnsc 0:9c001c4e7bf4 45 uint8_t checksumChar = checksum & 0x0F;
stearnsc 0:9c001c4e7bf4 46 if (checksumChar >= 10){
stearnsc 0:9c001c4e7bf4 47 checksumChar -= 10;
stearnsc 0:9c001c4e7bf4 48 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 49 } else {
stearnsc 0:9c001c4e7bf4 50 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 51 }
stearnsc 0:9c001c4e7bf4 52 checkSumString.push_back((char) checksumChar);
stearnsc 0:9c001c4e7bf4 53 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 54 }
stearnsc 0:9c001c4e7bf4 55
stearnsc 0:9c001c4e7bf4 56 for (int i = checkSumString.length() - 1; i >= 0; i--){
stearnsc 0:9c001c4e7bf4 57 gps.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 58 pc.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 59 }
stearnsc 0:9c001c4e7bf4 60 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 61 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 62 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 63 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 64 }
stearnsc 0:9c001c4e7bf4 65
stearnsc 0:9c001c4e7bf4 66 long parseDec(string s){
stearnsc 0:9c001c4e7bf4 67 int mult = 1;
stearnsc 0:9c001c4e7bf4 68 int result = 0;
stearnsc 0:9c001c4e7bf4 69 for (int i = s.length() - 1; i >=0; i--){
stearnsc 0:9c001c4e7bf4 70 if (s[i] != '.'){
stearnsc 0:9c001c4e7bf4 71 result += (s[i] - '0') * mult;
stearnsc 0:9c001c4e7bf4 72 mult *= 10;
stearnsc 0:9c001c4e7bf4 73 }
stearnsc 0:9c001c4e7bf4 74 }
stearnsc 0:9c001c4e7bf4 75 return result;
stearnsc 0:9c001c4e7bf4 76 }
stearnsc 0:9c001c4e7bf4 77
stearnsc 0:9c001c4e7bf4 78 //cs: little endian parsing
stearnsc 0:9c001c4e7bf4 79 int nextInt(char *data, int i){
stearnsc 0:9c001c4e7bf4 80 i |= data[i];
stearnsc 0:9c001c4e7bf4 81 i |= (data[i + 1] << 8);
stearnsc 0:9c001c4e7bf4 82 i |= (data[i + 2] << 16);
stearnsc 0:9c001c4e7bf4 83 i |= (data[i + 3] << 24);
stearnsc 0:9c001c4e7bf4 84 return i;
stearnsc 0:9c001c4e7bf4 85 }
stearnsc 0:9c001c4e7bf4 86
stearnsc 0:9c001c4e7bf4 87 void handleXbeeGps(){
stearnsc 0:9c001c4e7bf4 88 static bool reading = false;
stearnsc 0:9c001c4e7bf4 89 static char packet[16];
stearnsc 0:9c001c4e7bf4 90 static int i = 0;
stearnsc 0:9c001c4e7bf4 91
stearnsc 0:9c001c4e7bf4 92 char c = xbee.getc();
stearnsc 0:9c001c4e7bf4 93 if (reading){
stearnsc 0:9c001c4e7bf4 94 packet[i] = c;
stearnsc 0:9c001c4e7bf4 95 i++;
stearnsc 0:9c001c4e7bf4 96 if (i == 16){
stearnsc 0:9c001c4e7bf4 97 i = 0;
stearnsc 0:9c001c4e7bf4 98 otherGps.latitude = nextInt(packet, 0);
stearnsc 0:9c001c4e7bf4 99 otherGps.longitude = nextInt(packet, 4);
stearnsc 0:9c001c4e7bf4 100 otherGps.altitude = nextInt(packet, 8);
stearnsc 0:9c001c4e7bf4 101 otherGps.time = nextInt(packet, 12);
stearnsc 0:9c001c4e7bf4 102
stearnsc 0:9c001c4e7bf4 103 pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 0:9c001c4e7bf4 104 otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 0:9c001c4e7bf4 105 );
stearnsc 0:9c001c4e7bf4 106 reading = false;
stearnsc 0:9c001c4e7bf4 107 }
stearnsc 0:9c001c4e7bf4 108 } else if (c == 'X'){
stearnsc 0:9c001c4e7bf4 109 reading = true;
stearnsc 0:9c001c4e7bf4 110 }
stearnsc 0:9c001c4e7bf4 111 }
stearnsc 0:9c001c4e7bf4 112
stearnsc 0:9c001c4e7bf4 113 void handleGpsData(){
stearnsc 0:9c001c4e7bf4 114 static bool reading = false;
stearnsc 0:9c001c4e7bf4 115 static stringstream line;
stearnsc 0:9c001c4e7bf4 116
stearnsc 0:9c001c4e7bf4 117 char c = gps.getc();
stearnsc 0:9c001c4e7bf4 118
stearnsc 0:9c001c4e7bf4 119 if (reading){
stearnsc 0:9c001c4e7bf4 120 if (c == '*'){ //sentence buffer complete; we're ignoring the checksum
stearnsc 0:9c001c4e7bf4 121 string field;
stearnsc 0:9c001c4e7bf4 122 std::getline(line, field, ','); //GPGGA
stearnsc 0:9c001c4e7bf4 123 std::getline(line, field, ','); //time
stearnsc 0:9c001c4e7bf4 124 gpsData.time = parseDec(field);
stearnsc 0:9c001c4e7bf4 125 std::getline(line, field, ','); //latitude
stearnsc 0:9c001c4e7bf4 126 gpsData.latitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 127 std::getline(line, field, ','); //N or S
stearnsc 0:9c001c4e7bf4 128 std::getline(line, field, ','); //longitude
stearnsc 0:9c001c4e7bf4 129 gpsData.longitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 130 std::getline(line, field, ','); //E or W
stearnsc 0:9c001c4e7bf4 131 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 132 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 133 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 134 std::getline(line, field, ','); //altitude
stearnsc 0:9c001c4e7bf4 135 gpsData.altitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 136
stearnsc 0:9c001c4e7bf4 137 //update whatever needs updating when gps updates
stearnsc 0:9c001c4e7bf4 138 pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 0:9c001c4e7bf4 139 gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
stearnsc 0:9c001c4e7bf4 140 );
stearnsc 0:9c001c4e7bf4 141
stearnsc 0:9c001c4e7bf4 142 line.str(string(""));
stearnsc 0:9c001c4e7bf4 143 reading = false;
stearnsc 0:9c001c4e7bf4 144
stearnsc 0:9c001c4e7bf4 145 } else {
stearnsc 0:9c001c4e7bf4 146 line.put(c);
stearnsc 0:9c001c4e7bf4 147 }
stearnsc 0:9c001c4e7bf4 148
stearnsc 0:9c001c4e7bf4 149 } else if (c == '$') {
stearnsc 0:9c001c4e7bf4 150 reading = true;
stearnsc 0:9c001c4e7bf4 151 }
stearnsc 0:9c001c4e7bf4 152 }
dylanembed123 6:434d20e99e49 153 */
stearnsc 0:9c001c4e7bf4 154 int main(){
dylanembed123 6:434d20e99e49 155 //USB::getSerial().printf("------Starting Program------\n");
dylanembed123 6:434d20e99e49 156 Camera cam;
dylanembed123 6:434d20e99e49 157 char* version = cam.getVersion();
dylanembed123 6:434d20e99e49 158 //USB::getSerial().printf("Camera Version:\n%s\n",version);
dylanembed123 6:434d20e99e49 159 uint8_t targetSize=VC0706_640x480;//VC0706_160x120;
dylanembed123 6:434d20e99e49 160 cam.setImageSize(targetSize);
dylanembed123 6:434d20e99e49 161 uint8_t realSize=cam.getImageSize();
dylanembed123 6:434d20e99e49 162 //USB::getSerial().printf("Image Size | Target: %d, Real %d",targetSize,realSize);
dylanembed123 6:434d20e99e49 163 if (! cam.takePicture()) {
dylanembed123 6:434d20e99e49 164 USB::getSerial().printf("Failed to snap!\n");
dylanembed123 6:434d20e99e49 165 while(1){}
dylanembed123 6:434d20e99e49 166 } else {
dylanembed123 6:434d20e99e49 167 //USB::getSerial().printf("Picture taken!\n");
dylanembed123 6:434d20e99e49 168 }
dylanembed123 6:434d20e99e49 169 int size=cam.frameLength();
dylanembed123 6:434d20e99e49 170 //USB::getSerial().printf("Image Size %d\n",size);
dylanembed123 6:434d20e99e49 171 int i;
dylanembed123 6:434d20e99e49 172 for(i=0;i<size;){
dylanembed123 6:434d20e99e49 173 // read 32 bytes at a time;
dylanembed123 6:434d20e99e49 174 uint8_t bytesToRead = min(64, size-i); // change 32 to 64 for a speedup but may not work with all setups!
dylanembed123 6:434d20e99e49 175 uint8_t bytesRead=0;
dylanembed123 6:434d20e99e49 176 uint8_t* buffer = cam.readPicture(bytesToRead,&bytesRead);
dylanembed123 6:434d20e99e49 177 for(int a=0;a<bytesRead;a++){USB::getSerial().putc(buffer[a]);}
dylanembed123 6:434d20e99e49 178 //USB::getSerial().write(buffer,bytesToRead);
dylanembed123 6:434d20e99e49 179 //USB::getSerial().printf("Read: %d/%d - %d/%d\n",i,size,bytesRead,bytesToRead);
dylanembed123 6:434d20e99e49 180 i+=bytesRead;
dylanembed123 6:434d20e99e49 181 }
dylanembed123 6:434d20e99e49 182 //USB::getSerial().printf("Done(%d)\n",i);
dylanembed123 6:434d20e99e49 183 while(1){
dylanembed123 6:434d20e99e49 184 }
dylanembed123 6:434d20e99e49 185 // gps.baud(57600);
dylanembed123 6:434d20e99e49 186 // xbee.baud(9600);
dylanembed123 6:434d20e99e49 187 // pc.baud(57600);
stearnsc 0:9c001c4e7bf4 188
stearnsc 0:9c001c4e7bf4 189 // sendGpsCommand("PMTK301,1");
stearnsc 0:9c001c4e7bf4 190 // while(true){pc.putc(gps.getc());}
dylanembed123 6:434d20e99e49 191 // gps.attach(&handleGpsData, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 192 // xbee.attach(&handleXbeeGps, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 193 }