Revision 14:6be57da62283, committed 2014-04-10

Comitter:

dylanembed123

Date:

Thu Apr 10 02:19:07 2014 +0000

Parent:

13:a6d3cf2b018e

Child:

15:e3e03a9df89e

Commit message:

Update GPS;

Changed in this revision

--- a/adapt/camera.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/camera.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -247,11 +247,11 @@
   frameptr = 0;
   return cameraFrameBuffCtrl(VC0706_STOPCURRENTFRAME); 
 }
-/*
-boolean Camera::resumeVideo() {
+
+bool Camera::resumeVideo() {
   return cameraFrameBuffCtrl(VC0706_RESUMEFRAME); 
 }
-
+/*
 boolean Camera::TVon() {
   uint8_t args[] = {0x1, 0x1};
   return runCommand(VC0706_TVOUT_CTRL, args, sizeof(args), 5);

--- a/adapt/camera.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/camera.h	Thu Apr 10 02:19:07 2014 +0000
@@ -68,6 +68,7 @@
     char * getVersion(void);
     
     bool takePicture();
+    bool resumeVideo();
     bool cameraFrameBuffCtrl(uint8_t command);
     uint8_t* readPicture(uint8_t n,uint8_t* outSize=NULL);
     uint32_t frameLength(void);

--- a/adapt/gps.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/gps.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -9,4 +9,4 @@
         gps->baud(GPSBAUD);
     }
     return *gps;
-}
\ No newline at end of file
+}

--- a/adapt/gps.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/gps.h	Thu Apr 10 02:19:07 2014 +0000
@@ -17,4 +17,5 @@
     /// \brief Call this function to get a hold of the serial for USB.
     static Serial& getSerial();
 };
+
 #endif
\ No newline at end of file

--- a/adapt/usb.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/usb.h	Thu Apr 10 02:19:07 2014 +0000
@@ -4,6 +4,7 @@
 
 /// Define Baud
 #define USBBAUD 115200
+//115200
 
 class USB{
 private:

--- a/adapt/xbee.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/xbee.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -1,5 +1,4 @@
 #include "xbee.h"
-
 Serial* XBEE::xbee=NULL;
 
 Serial& XBEE::getSerial(){
@@ -7,6 +6,7 @@
         // Init Serial USB
         xbee=new Serial(XBEEPINTX,XBEEPINRX);
         xbee->baud(XBEEBAUD);
+        //xbee->set_flow_control(SerialBase::CTS,NC,p20);
     }
     return *xbee;
 }
\ No newline at end of file

--- a/adapt/xbee.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/adapt/xbee.h	Thu Apr 10 02:19:07 2014 +0000
@@ -8,7 +8,7 @@
 #define XBEEPINRX p10
 
 /// Define Baud
-#define XBEEBAUD 9600
+#define XBEEBAUD 115200
 
 class XBEE{
 private:

--- a/handle/dataLocation.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/handle/dataLocation.h	Thu Apr 10 02:19:07 2014 +0000
@@ -2,7 +2,12 @@
   *  \brief Data location data holder
   **/
 
-#define MAXNUMLOCS 256
+#define MAXNUMLOCS 64
+
+typedef struct DataLocS{
+    double lat,lon,alt;
+    double timestamp;
+}DataLoc;
 
 // Storage of data location
 class DataLocation{

--- a/handle/handleCamera.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/handle/handleCamera.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -3,7 +3,7 @@
 void ImageHandle::setup(){
     char* version = cam.getVersion();
     outputDevice.printf("Version %s\n",version);
-    uint8_t targetSize=VC0706_640x480;
+    uint8_t targetSize=VC0706_320x240;//640x480;
     // 320x240
     //VC0706_640x480;//VC0706_160x120;
     cam.setImageSize(targetSize);
@@ -16,6 +16,7 @@
         while(1){}
     }
     unsigned int sID=getPS().getSuperID();
+    getPS().sendPacket(0,NULL,0,PT_EMPTY);
     getPS().sendPacket(sID,NULL,0,PT_IMAGE);
     int size=cam.frameLength();
     outputDevice.printf("Image Start %d\n",size);
@@ -41,6 +42,7 @@
     getPS().sendPacket(sID,backBuffer,bloc);
     outputDevice.printf("Image End\n",size);
     getPS().sendPacket(sID,NULL,0,PT_END);
+    cam.resumeVideo();
 }
 
 bool ImageHandle::check(){
@@ -55,6 +57,7 @@
     }
     if(check()){
         take();
-        while(true){}
+        wait(2);
+        //while(true){}
     }
 }
\ No newline at end of file

--- a/handle/handleGPS.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/handle/handleGPS.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -2,70 +2,93 @@
 
 //Serial gps(p28,p27);
 
+void testVar(){
+}
 void GPSHandle::setup(){
-    gps.getSerial().baud(57600);
+    //gpsGPS().getSerial().baud(57600);
     sendGpsCommand("PMTK301,1");
-    //gps.getSerial().attach(this, handleUpdate, Serial::RxIrq);
+    //GPS::getSerial().attach(&GPSHandle::handleUpdate,Serial::RxIrq);
+    GPS::getSerial().attach(this,&GPSHandle::handleUpdate,Serial::RxIrq);
     //cs: Send other standard init commands? Not strictly speaking necessary,
     //    but forces "up to date documentation" in the form of always knowing
     //    gps config.
 }
 
+static bool reading = false;
+//static std::stringstream line;
+static char line[MAXREADIN+10];
+static int line_i=0;
+char* getNext(char*& field){
+    char* output=new char[MAXREADIN+1];
+    int i;
+    for(i=0;i<MAXREADIN;i++){
+        if(field[i]=='\0'||field[i]==',')break;
+    }
+    for(int a=0;a<i;a++){
+        output[a]=field[a];
+    }
+    output[i]='\0';
+    field=&field[i+1];
+    return output;
+}
+    
 void GPSHandle::handleUpdate(){
-    static bool reading = false;
-    static std::stringstream line;
-
-    char c = gps.getSerial().getc();
-
+    if(GPS::getSerial().readable()<=0){return;}
+    char c = GPS::getSerial().getc();
+    //USB::getSerial().printf("%c",c);
     if (reading) {
+        if(line_i>=MAXREADIN){reading=false;return;}
         if (c == '*') { //sentence buffer complete; we're ignoring the checksum
-            std::string field;
-            std::getline(line, field, ','); //GPGGA
-            std::getline(line, field, ','); //time
-            double time = atof(field.c_str());
-            std::getline(line, field, ','); //latitude
-            double latitude = atof(field.c_str());
-            std::getline(line, field, ','); //N or S
-            std::getline(line, field, ','); //longitude
-            double longitude = atof(field.c_str());
-            std::getline(line, field, ','); //E or W
-            std::getline(line, field, ','); //skip
-            std::getline(line, field, ','); //skip
-            std::getline(line, field, ','); //skip
-            std::getline(line, field, ','); //altitude
-            double altitude = atof(field.c_str());
+            char* field=line;
+            char* op;
+            op=getNext(field);delete op; //GPGGA
+            if(op[0]=='G'||op[1]=='P'||op[2]=='G'||op[3]=='G'||op[4]=='A'){
+            op=getNext(field);double timeS = atof(op);delete op; //time
+            op=getNext(field);double latitude = atof(op);delete op; //latitude
+            op=getNext(field);delete op; //N or S
+            op=getNext(field);double longitude = atof(op);delete op; //longitude
+            op=getNext(field);delete op; //E or W
+            op=getNext(field);delete op; //skip
+            op=getNext(field);delete op; //skip
+            op=getNext(field);delete op; //skip
+            op=getNext(field);delete op; //altitude
+            double altitude = atof(op);
 
+            USB::getSerial().printf("\nMy GPS data: Lat: %f, Lon: %f, Alt: %f, Time:%f\r\n",latitude,longitude,altitude,timeS);
+            Locs().add(LHType_locs,DataLocation(latitude,longitude,altitude,timeS));
+            }
             //update whatever needs updating when gps updates
 //            pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
 //                      gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
 //                     );
 
-            line.str(string(""));
             reading = false;
-
         } else {
-            line.put(c);
+            line[line_i]=c;
+            line_i=(line_i+1)%MAXREADIN;
         }
 
     } else if (c == '$') {
         reading = true;
+        line_i=0;
     }
+    return;
 }
 
 //sends: "$<command>*<checksum>\r\l"
 void GPSHandle::sendGpsCommand(std::string command){
     uint8_t checksum = 0;
     //pc.printf("Sending command to gps: ");
-    gps.getSerial().putc('$');
+    GPS::getSerial().putc('$');
     //pc.putc('$');
     char c;
     for (int i = 0; i < command.length(); i++) {
         c = command[i];
         checksum ^= c;
-        gps.getSerial().putc(c);
+        GPS::getSerial().putc(c);
         //pc.putc(c);
     }
-    gps.getSerial().putc('*');
+    GPS::getSerial().putc('*');
     //pc.putc('*');
     string checkSumString;
     while (checksum > 0) {
@@ -81,12 +104,12 @@
     }
 
     for (int i = checkSumString.length() - 1; i >= 0; i--) {
-        gps.getSerial().putc(checkSumString[i]);
+        GPS::getSerial().putc(checkSumString[i]);
         //pc.putc(checkSumString[i]);
     }
-    gps.getSerial().putc('\r');
+    GPS::getSerial().putc('\r');
     //pc.putc('\r');
-    gps.getSerial().putc('\n');
+    GPS::getSerial().putc('\n');
     //pc.putc('\n');
 }
 
@@ -105,4 +128,7 @@
 
 bool GPSHandle::check(){
     return true;
+}
+void GPSHandle::run(){
+    if(!initialized){initialized=true;setup();}
 }
\ No newline at end of file

--- a/handle/handleGPS.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/handle/handleGPS.h	Thu Apr 10 02:19:07 2014 +0000
@@ -1,15 +1,20 @@
-#ifndef _HANDLEGPS_H_
-#define _HANDLEGPS_H_
+#ifndef _HANDLE_GPS_H_
+#define _HANDLE_GPS_H_
 
 #include "adapt/usb.h"
 #include "adapt/gps.h"
-#ifndef _HANDLE_GPS_H_
-#define _HANDLE_GPS_H_
+
 #include <string>
 #include <sstream>
+#include "dataLocation.h"
+#include <mbed.h>
+#include "gps.h"
+
+#define MAXREADIN 256
+
 class GPSHandle{
 private:
-    GPS gps;
+    //GPS gps;
     bool initialized;
     
     /// \brief Setup
@@ -28,5 +33,5 @@
     /// \brief Run an instance of this
     void run();
 };
-#endif
+
 #endif
\ No newline at end of file

--- a/main.cpp	Mon Apr 07 01:30:04 2014 +0000
+++ b/main.cpp	Thu Apr 10 02:19:07 2014 +0000
@@ -103,6 +103,29 @@
 
 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);
+    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;
@@ -112,11 +135,12 @@
         //USB::getSerial().printf("Test\n");
         //XBEE::getSerial().printf("ABC\n");
     //}
+    USB::getSerial().printf("Check GPS\n");
     while(1){
         // Run image handler
-        imageHand.run();
+        //imageHand.run();
         // Run GPS handler
-        //gpsHand.run();
+        gpsHand.run();
     }
     /// Main Loop
     while(true) {

--- a/packet.h	Mon Apr 07 01:30:04 2014 +0000
+++ b/packet.h	Thu Apr 10 02:19:07 2014 +0000
@@ -5,9 +5,10 @@
 // Packet 2. (SuperPackid=5,type=PT_DEFAULT,size=1024)
 // Packet 3. (SuperPackid=5,type=PT_DEFAULT,size=1000)
 // Packet 4. (SuperPackid=5,type=PT_END,size=0)
-#define XBEEON
+//#define XBEEON
 enum PACKET_TYPE{
-    PT_DEFAULT=0,
+    PT_EMPTY=0,
+    PT_DEFAULT,
     PT_END,
     PT_IMAGE,
     PT_SIZE
@@ -17,6 +18,7 @@
     unsigned int size;// Number of valid bits
     char data[PACKETSIZE];
     unsigned int superPackID;// 
+    char special[4];// Set to FF when
 }PacketStruct;
 
 class PacketSender{
@@ -30,13 +32,16 @@
         #else
         USB::getSerial()
         #endif
-    ){}
+    ),next(NULL){}
     Serial& outputDevice;
     void sendPacket(PacketStruct& output){
         for(int a=0;a<sizeof(PacketStruct);a++){
+            while(!outputDevice.writeable()){}
             outputDevice.putc(((char*)(&output))[a]);
+            //wait_ms(10);
             //USB::getSerial().putc(((char*)(&output))[a]);
         }
+        //wait_ms(100);
     }
     unsigned int min(unsigned int a,unsigned int b){
         return a<b ? a : b;
@@ -48,25 +53,70 @@
                 output.type=PT_DEFAULT;
                 output.superPackID=superPackID;
                 output.size=min(PACKETSIZE,size-i*PACKETSIZE);
-                for(int a=0;a<output.size;a++){
-                    //USB::getSerial().printf(">>%d/%d - %d\n",a,size,output.size);
-                    output.data[a]=data[a-i*PACKETSIZE];
-                }
+                for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
+                for(int a=0;a<output.size;a++){output.data[a]=data[a-i*PACKETSIZE];}
                 for(int a=output.size;a<PACKETSIZE;a++){output.data[a]='\0';}
-                //memcpy(output.data,&(data[i*PACKETSIZE]),min(PACKETSIZE,size-i*PACKETSIZE));
                 sendPacket(output);
             }
-            
         }else{
             PacketStruct output;
             output.type=type;
             output.size=0;
             output.superPackID=superPackID;
-            for(int a=0;a<PACKETSIZE;a++){output.data[a]=a;}
+            for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
+            // Check for empty packet
+            if(output.type==PT_EMPTY){output.type=0;output.size=0;output.superPackID=0;output.special[3]=0xFF;}
+            for(int a=0;a<PACKETSIZE;a++){output.data[a]='\0';}
             sendPacket(output);
         }
     }
+    
+    // Number of consecutive zeros
+    unsigned int numZeros;
+    // Return true if a resync command has been received
+    bool resetCheck(char input){
+        if(input=='\0'){
+            numZeros++;
+        }else if(numZeros==sizeof(PacketStruct)-1&&input==0xFF){
+            return true;
+        }else{
+            numZeros=0;
+        }
+        return false;
+    }
+    
+    // Temperary storage for next valid
+    PacketStruct* next;
+    // Number of valid bits in next packet
+    int nextValid;
+    /// \brief Grab the next packet
     PacketStruct* getNextPacket(){
+        // Check for null packet
+        if(next==NULL){next=new PacketStruct();nextValid=0;}
+        // Create reset packet which resets on re-sync command
+        bool resetPacket=false;
+
+        // While there is data to read
+        while(0<outputDevice.readable()){
+            // Check if a full packet has been received
+            if(nextValid==sizeof(PacketStruct))break;
+            // Read in next char
+            char input=outputDevice.getc();
+            //USB::getSerial().printf("Read ByteC %X %X\n",nextValid,input);
+            // Check for valid char
+            if(resetCheck(input)){resetPacket=true;break;}
+            // Set char
+            ((char*)next)[nextValid++] = input;
+        }
+        
+        if(nextValid==sizeof(PacketStruct)||resetPacket){
+            // Reset packet
+            PacketStruct* output=next;next=NULL;
+            // Return
+            return resetPacket?NULL:output;
+        }
+        return NULL;
+/*        
         int avail = outputDevice.readable();
         if(avail <= 0)return NULL;
         PacketStruct* output=new PacketStruct();
@@ -76,6 +126,7 @@
             ((char*)output)[i] = outputDevice.getc();
         }
         return output;
+*/
     }
     
 };