Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
main.cpp
- Committer:
- krobertson
- Date:
- 2014-04-20
- Revision:
- 18:e72ee7aed088
- Parent:
- 16:4f5d20b87dc3
- Child:
- 19:8c1f2a2204fb
File content as of revision 18:e72ee7aed088:
#include "mbed.h" #include <string> #include <sstream> #include "adapt/usb.h" #include "handle/handleCamera.h" #include "handle/handleGPS.h" Serial pc(USBTX,USBRX); Serial xbee(p9,p10);//tx, rx Serial gps(p28,p27); Serial camera(p13,p14); typedef struct { int latitude; //in .0001 minutes int longitude; //in .0001 minutes int altitude; //in decimeters int time; //in milliseconds } GpsData; void readSerial(Serial &s, char str[], int size) { for (int i = 0; i < size; i++) { str[i] = s.getc(); } } void connection_lost(){ USB::getSerial().printf("TCP connection lost!\r\n"); } //sends: "$<command>*<checksum>\r\l" void sendGpsCommand(string command) { uint8_t checksum = 0; pc.printf("Sending command to gps: "); gps.putc('$'); pc.putc('$'); char c; for (int i = 0; i < command.length(); i++) { c = command[i]; checksum ^= c; gps.putc(c); pc.putc(c); } gps.putc('*'); pc.putc('*'); string checkSumString; while (checksum > 0) { uint8_t checksumChar = checksum & 0x0F; if (checksumChar >= 10) { checksumChar -= 10; checksumChar += 'A'; } else { checksumChar += '0'; } checkSumString.push_back((char) checksumChar); checksum = checksum >> 4; } for (int i = checkSumString.length() - 1; i >= 0; i--) { gps.putc(checkSumString[i]); pc.putc(checkSumString[i]); } gps.putc('\r'); pc.putc('\r'); gps.putc('\n'); pc.putc('\n'); } // ////cs: little endian parsing //int nextInt(char *data, int i) //{ // i |= data[i]; // i |= (data[i + 1] << 8); // i |= (data[i + 2] << 16); // i |= (data[i + 3] << 24); // return i; //} //void handleXbeeGps() //{ // static bool reading = false; // static char packet[16]; // static int i = 0; // // char c = xbee.getc(); // if (reading) { // packet[i] = c; // i++; // if (i == 16) { // i = 0; // otherGps.latitude = nextInt(packet, 0); // otherGps.longitude = nextInt(packet, 4); // otherGps.altitude = nextInt(packet, 8); // otherGps.time = nextInt(packet, 12); // // pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", // otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time // ); // reading = false; // } // } else if (c == 'X') { // reading = true; // } //} int main() { //getPS().sendPacket(0,NULL,0,PT_EMPTY);getPS().sendPacket(55,NULL,0,PT_IMAGE); //char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(0,NULL,0,PT_IMAGE);getPS().sendPacket(55,output,256);getPS().sendPacket(55,output,5);getPS().sendPacket(55,NULL,0,PT_END); //while(true){} /* PacketStruct* packs[4]; for(int i=0;i<4;i){ packs[i]=getPS().getNextPacket(); if(packs[i]!=NULL)i++; } for(int i=0;i<4;i++){ PacketStruct* pack=packs[i]; if(pack!=NULL){ USB::getSerial().printf("Got Packet!\n"); USB::getSerial().printf(" > %d\n",pack->type); for(int i=0;i<sizeof(PacketStruct);i++){ USB::getSerial().printf("%d\n",((char*)pack)[i]); } USB::getSerial().printf("\n",pack->type); }else{ //USB::getSerial().printf("."); } } while(true){} */ //getPS().sendPacket(55,NULL,0,PT_IMAGE);char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(55,output,256,PT_IMAGE);getPS().sendPacket(55,output,5,PT_IMAGE);getPS().sendPacket(55,NULL,0,PT_END);while(true){} ImageHandle imageHand; GPSHandle gpsHand; /// Main Loop USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct)); //while(1){ //USB::getSerial().printf("Test\n"); //XBEE::getSerial().printf("ABC\n"); //} USB::getSerial().printf("Check GPS\n"); USB::getSerial().printf("Connect to the wifly network now!\r\n"); __disable_irq(); XBEE::getTCPInterrupt().fall(&connection_lost); getPS().wait_for_egg(); //for(int a=0;a<1000000000;a++); while(1){ //while(1){ // USB::getSerial().printf("sending 0's\n"); // getPS().openConnection(); // USB::getSerial().printf("connection open!\r\n"); // for(int b=0;b<50;b++){ // for(int a=0;a<sizeof(PacketStruct)-1;a++){ // while(!getPS().outputDevice.writeable()){} // getPS().outputDevice.putc(0); // } // while(!getPS().outputDevice.writeable()){} // getPS().outputDevice.putc(0xFF); // } // USB::getSerial().printf("sent all data\r\n"); // //for(int a=0;a<10000000;a++); // getPS().closeConnection(); // //for(int a=0;a<10000000;a++); // } // Run image handler USB::getSerial().printf("Check Image\n"); imageHand.run(); // Run GPS handler USB::getSerial().printf("Check GPS\n"); gpsHand.run(); USB::getSerial().printf("GPS Time: %f\n",DH::Locs().getC().getTime()); // Read packet USB::getSerial().printf("Read Input\n"); //PacketStruct* pack=getPS().lastValid;//getPS().getNextPacket(); //if(pack!=NULL){ // USB::getSerial().printf("Received Type: %d\n",pack->type); // if(pack->type==PT_REQLOC){ if(getPS().outputDevice.readable()>0){ char input=getPS().outputDevice.getc(); //if(getPS().outputDevice.getc()=='A'){ // Send location unsigned int sID=getPS().getSuperID(); getPS().sendPacket(0,NULL,0,PT_EMPTY); getPS().sendPacket(sID,NULL,0,PT_SENDLOC); getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation)); getPS().sendPacket(sID,NULL,0,PT_END); //} } } /// Main Loop while(true) { gps.baud(57600); xbee.baud(9600); pc.baud(57600); sendGpsCommand("PMTK301,1"); while(true) { pc.putc(gps.getc()); } //gps.attach(&handleGpsData, Serial::RxIrq); //xbee.attach(&handleXbeeGps, Serial::RxIrq)//; } }