Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
main.cpp
- Committer:
- dylanembed123
- Date:
- 2014-04-23
- Revision:
- 34:c0b13ce5408c
- Parent:
- 32:9cb7bc3fc9e0
- Parent:
- 33:ad63e7013801
- Child:
- 35:a6177e5ca00c
File content as of revision 34:c0b13ce5408c:
#include "mbed.h" #include <string> #include <sstream> #include "adapt/usb.h" #include "handle/handleCamera.h" #include "handle/handleGPS.h" #include "handle/handleCommand.h" #include "handle/mavcommands.h" #include "handle/dataLocation.h" #define DELAYBOOT 1 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() { // Start Mav test USB::getSerial().printf("Wait 20\n"); wait(DELAYBOOT); while(true){ MavCmd::get().run(); } // End mav test //handlers //ImageHandle imageHand; //GPSHandle gpsHand; //CommandHandle commHand; USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct)); USB::getSerial().printf("Check GPS\n"); USB::getSerial().printf("Connect to the wifly network now!\r\n"); //XBEE::getTCPInterrupt().fall(&connection_lost); //checking connection to egg before continuing getPS().openConnection(); getPS().closeConnection(); //Main Loop while(1){ GPSHandle::getGPSHand().run(); USB::getSerial().printf("Requesting commands from egg...\r\n"); wait_us(100000); CommandHandle::getCommandHand().run(); wait_us(100000); if(GPSHandle::getGPSHand().if_image_location()){ USB::getSerial().printf("Taking picture and sending...\r\n"); wait_us(100000); ImageHandle::getImageHand().run(); USB::getSerial().printf("sent all data\r\n"); wait_us(100000); GPSHandle::getGPSHand().next_waypoint(); }else{ USB::getSerial().printf("Not close enough to waypoint for image\r\n"); } wait_us(1000000); //} // 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)//; //} }