Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
main.cpp
- Committer:
- dylanembed123
- Date:
- 2014-05-05
- Revision:
- 66:5d43988d100c
- Parent:
- 64:d4818fb7813c
File content as of revision 66:5d43988d100c:
#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" #include "handle/handleCompass.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 //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0)); USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT); wait(DELAYBOOT); //while(true){ // USB::getSerial().printf(".\n",DELAYBOOT); // MavCmd::get().run(); // compassHandle::getCompassHand().run(); // wait(5); //} 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 int count = 0; while(1){ //USB::getSerial().printf("Running GPS...\r\n"); //GPSHandle::getGPSHand().run(); MavCmd::get().run(); //USB::getSerial().printf("Running Compass...\r\n"); compassHandle::getCompassHand().run(); if(count % 10 == 0){ 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(); wait_us(100000); }else{ //USB::getSerial().printf("Not close enough to waypoint for image\r\n"); } count++; } }