Revision 7:c75d5e5e6bfc, committed 2014-04-01

Comitter:

dylanembed123

Date:

Tue Apr 01 15:52:08 2014 +0000

Parent:

6:434d20e99e49

Child:

8:28b866df62cf

Commit message:

Update handler and adapter;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/camera.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,372 @@
+/*************************************************** 
+  This is a library for the Adafruit TTL JPEG Camera (VC0706 chipset)
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/products/397
+
+  These displays use Serial to communicate, 2 pins are required to interface
+
+  Adafruit invests time and resources providing this open source code, 
+  please support Adafruit and open-source hardware by purchasing 
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.  
+  BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+
+#include "camera.h"
+
+Serial* Camera::camera=NULL;
+uint8_t Camera::serialNum=0;
+
+Serial& Camera::getSerial(){
+    if(camera==NULL){
+        // Init Camera
+        camera=new Serial(CAMERAPINTX,CAMERAPINRX);
+        camera->baud(CAMERABAUD);
+        serialNum=0;
+        Camera temp;
+        temp.reset();
+    }
+    return *camera;
+}
+
+bool Camera::reset() {
+  uint8_t args[] = {0x0};
+  return runCommand(VC0706_RESET, args, 1, 5);
+}
+
+/*
+// Initialization code used by all constructor types
+void Camera::common_init(void) {
+  swSerial  = NULL;
+  hwSerial  = NULL;
+  frameptr  = 0;
+  bufferLen = 0;
+  serialNum = 0;
+}
+
+// Constructor when using SoftwareSerial or NewSoftSerial
+#if ARDUINO >= 100
+Camera::Camera(SoftwareSerial *ser) {
+#else
+Camera::Camera(NewSoftSerial *ser) {
+#endif
+  common_init();  // Set everything to common state, then...
+  swSerial = ser; // ...override swSerial with value passed.
+}
+
+// Constructor when using HardwareSerial
+Camera::Camera(HardwareSerial *ser) {
+  common_init();  // Set everything to common state, then...
+  hwSerial = ser; // ...override hwSerial with value passed.
+}
+
+boolean Camera::begin(uint16_t baud) {
+  if(swSerial) swSerial->begin(baud);
+  else         hwSerial->begin(baud);
+  return reset();
+}
+
+
+boolean Camera::motionDetected() {
+  if (readResponse(4, 200) != 4) {
+    return false;
+  }
+  if (! verifyResponse(VC0706_COMM_MOTION_DETECTED))
+    return false;
+
+  return true;
+}
+
+
+uint8_t Camera::setMotionStatus(uint8_t x, uint8_t d1, uint8_t d2) {
+  uint8_t args[] = {0x03, x, d1, d2};
+
+  return runCommand(VC0706_MOTION_CTRL, args, sizeof(args), 5);
+}
+
+
+uint8_t Camera::getMotionStatus(uint8_t x) {
+  uint8_t args[] = {0x01, x};
+
+  return runCommand(VC0706_MOTION_STATUS, args, sizeof(args), 5);
+}
+
+
+boolean Camera::setMotionDetect(boolean flag) {
+  if (! setMotionStatus(VC0706_MOTIONCONTROL, 
+            VC0706_UARTMOTION, VC0706_ACTIVATEMOTION))
+    return false;
+
+  uint8_t args[] = {0x01, flag};
+  
+  runCommand(VC0706_COMM_MOTION_CTRL, args, sizeof(args), 5);
+}
+
+
+
+boolean Camera::getMotionDetect(void) {
+  uint8_t args[] = {0x0};
+
+  if (! runCommand(VC0706_COMM_MOTION_STATUS, args, 1, 6))
+    return false;
+
+  return camerabuff[5];
+}
+*/
+uint8_t Camera::getImageSize() {
+  uint8_t args[] = {0x4, 0x4, 0x1, 0x00, 0x19};
+  if (! runCommand(VC0706_READ_DATA, args, sizeof(args), 6))
+    return 0xFF;
+
+  return camerabuff[5];
+}
+
+bool Camera::setImageSize(uint8_t x) {
+  uint8_t args[] = {0x05, 0x04, 0x01, 0x00, 0x19, x};
+  return runCommand(VC0706_WRITE_DATA, args, sizeof(args), 5);
+}
+/*
+// downsize image control 
+
+uint8_t Camera::getDownsize(void) {
+  uint8_t args[] = {0x0};
+  if (! runCommand(VC0706_DOWNSIZE_STATUS, args, 1, 6)) 
+    return -1;
+
+   return camerabuff[5];
+}
+
+boolean Camera::setDownsize(uint8_t newsize) {
+  uint8_t args[] = {0x01, newsize};
+
+  return runCommand(VC0706_DOWNSIZE_CTRL, args, 2, 5);
+}
+
+// other high level commands 
+*/
+char * Camera::getVersion(void) {
+  uint8_t args[] = {0x01};
+  
+  sendCommand(VC0706_GEN_VERSION, args, 1);
+  // get reply
+  if (!readResponse(CAMERABUFFSIZ, 200)){
+    return 0;
+  }
+  camerabuff[bufferLen] = 0;  // end it!
+  return (char *)camerabuff;  // return it!
+}
+/*
+
+// high level photo comamnds
+
+void Camera::OSD(uint8_t x, uint8_t y, char *str) {
+  if (strlen(str) > 14) { str[13] = 0; }
+
+  uint8_t args[17] = {strlen(str), strlen(str)-1, (y & 0xF) | ((x & 0x3) << 4)};
+
+  for (uint8_t i=0; i<strlen(str); i++) {
+    char c = str[i];
+    if ((c >= '0') && (c <= '9')) {
+      str[i] -= '0';
+    } else if ((c >= 'A') && (c <= 'Z')) {
+      str[i] -= 'A';
+      str[i] += 10;
+    } else if ((c >= 'a') && (c <= 'z')) {
+      str[i] -= 'a';
+      str[i] += 36;
+    }
+
+    args[3+i] = str[i];
+  }
+
+   runCommand(VC0706_OSD_ADD_CHAR, args, strlen(str)+3, 5);
+   printBuff();
+}
+
+boolean Camera::setCompression(uint8_t c) {
+  uint8_t args[] = {0x5, 0x1, 0x1, 0x12, 0x04, c};
+  return runCommand(VC0706_WRITE_DATA, args, sizeof(args), 5);
+}
+
+uint8_t Camera::getCompression(void) {
+  uint8_t args[] = {0x4, 0x1, 0x1, 0x12, 0x04};
+  runCommand(VC0706_READ_DATA, args, sizeof(args), 6);
+  printBuff();
+  return camerabuff[5];
+}
+
+boolean Camera::setPTZ(uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) {
+  uint8_t args[] = {0x08, wz >> 8, wz, 
+            hz >> 8, wz, 
+            pan>>8, pan, 
+            tilt>>8, tilt};
+
+  return (! runCommand(VC0706_SET_ZOOM, args, sizeof(args), 5));
+}
+
+
+boolean Camera::getPTZ(uint16_t &w, uint16_t &h, uint16_t &wz, uint16_t &hz, uint16_t &pan, uint16_t &tilt) {
+  uint8_t args[] = {0x0};
+  
+  if (! runCommand(VC0706_GET_ZOOM, args, sizeof(args), 16))
+    return false;
+  printBuff();
+
+  w = camerabuff[5];
+  w <<= 8;
+  w |= camerabuff[6];
+
+  h = camerabuff[7];
+  h <<= 8;
+  h |= camerabuff[8];
+
+  wz = camerabuff[9];
+  wz <<= 8;
+  wz |= camerabuff[10];
+
+  hz = camerabuff[11];
+  hz <<= 8;
+  hz |= camerabuff[12];
+
+  pan = camerabuff[13];
+  pan <<= 8;
+  pan |= camerabuff[14];
+
+  tilt = camerabuff[15];
+  tilt <<= 8;
+  tilt |= camerabuff[16];
+
+  return true;
+}
+
+*/
+bool Camera::takePicture() {
+  frameptr = 0;
+  return cameraFrameBuffCtrl(VC0706_STOPCURRENTFRAME); 
+}
+/*
+boolean Camera::resumeVideo() {
+  return cameraFrameBuffCtrl(VC0706_RESUMEFRAME); 
+}
+
+boolean Camera::TVon() {
+  uint8_t args[] = {0x1, 0x1};
+  return runCommand(VC0706_TVOUT_CTRL, args, sizeof(args), 5);
+}
+boolean Camera::TVoff() {
+  uint8_t args[] = {0x1, 0x0};
+  return runCommand(VC0706_TVOUT_CTRL, args, sizeof(args), 5);
+}
+*/
+bool Camera::cameraFrameBuffCtrl(uint8_t command) {
+  uint8_t args[] = {0x1, command};
+  return runCommand(VC0706_FBUF_CTRL, args, sizeof(args), 5);
+}
+
+uint32_t Camera::frameLength(void) {
+  uint8_t args[] = {0x01, 0x00};
+  if (!runCommand(VC0706_GET_FBUF_LEN, args, sizeof(args), 9))
+    return 0;
+
+  uint32_t len;
+  len = camerabuff[5];
+  len <<= 8;
+  len |= camerabuff[6];
+  len <<= 8;
+  len |= camerabuff[7];
+  len <<= 8;
+  len |= camerabuff[8];
+
+  return len;
+}
+/*
+
+uint8_t Camera::available(void) {
+  return bufferLen;
+}
+
+*/
+uint8_t * Camera::readPicture(uint8_t n,uint8_t* outSize) {
+  uint8_t args[] = {0x0C, 0x0, 0x0A, 
+                    0, 0, frameptr >> 8, frameptr & 0xFF, 
+                    0, 0, 0, n, 
+                    CAMERADELAY >> 8, CAMERADELAY & 0xFF};
+
+  if (! runCommand(VC0706_READ_FBUF, args, sizeof(args), 5, false))
+    return 0;
+
+
+  // read into the buffer PACKETLEN!
+  int outputSize=readResponse(n+5, CAMERADELAY)-5;
+  if (outputSize == 0) 
+      return 0;
+  if(outSize!=NULL)*outSize=outputSize;
+
+  frameptr += outputSize;
+
+  return camerabuff;
+}
+
+
+//
+// Low level commands
+//
+
+bool Camera::runCommand(uint8_t cmd, uint8_t *args, uint8_t argn, uint8_t resplen, bool flushflag) {
+  // flush out anything in the buffer?
+  if (flushflag) {
+    readResponse(100, 10); 
+  }
+
+  sendCommand(cmd, args, argn);
+  if (readResponse(resplen, 200) != resplen) 
+    return false;
+  if (! verifyResponse(cmd))
+    return false;
+  return true;
+}
+
+
+void Camera::sendCommand(uint8_t cmd, uint8_t args[], uint8_t argn) {
+    getSerial().putc(0x56);
+    getSerial().putc(serialNum);
+    getSerial().putc(cmd);
+    
+    for (uint8_t i=0; i<argn; i++) {
+      getSerial().putc(args[i]);
+    }
+}
+
+uint8_t Camera::readResponse(uint8_t numbytes, uint8_t timeout) {
+  uint16_t counter = 0;
+  bufferLen = 0;
+ 
+  while ((timeout*100 != counter) && (bufferLen != numbytes)){
+    int avail = getSerial().readable();
+    if (avail <= 0) {
+      wait_us(10);
+      counter++;
+      continue;
+    }
+    counter = 0;
+    // there's a byte!
+    camerabuff[bufferLen++] = getSerial().getc();
+  }
+  return bufferLen;
+}
+
+bool Camera::verifyResponse(uint8_t command) {
+  if ((camerabuff[0] != 0x76) ||
+      (camerabuff[1] != serialNum) ||
+      (camerabuff[2] != command) ||
+      (camerabuff[3] != 0x0))
+      return false;
+  return true;
+  
+}
+
+void Camera::printBuff() {
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/camera.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,91 @@
+/**
+  *  \brief Camera interface
+  **/
+#define CAMERABUFFSIZ 128
+#include "usb.h"
+#include "mbed.h"
+
+/// Define Pinout
+#define CAMERAPINTX p9
+#define CAMERAPINRX p10
+
+/// Define Baud
+#define CAMERABAUD 38400
+
+/// Define commands
+#define VC0706_RESET  0x26
+#define VC0706_GEN_VERSION 0x11
+#define VC0706_READ_FBUF 0x32
+#define VC0706_GET_FBUF_LEN 0x34
+#define VC0706_FBUF_CTRL 0x36
+#define VC0706_DOWNSIZE_CTRL 0x54
+#define VC0706_DOWNSIZE_STATUS 0x55
+#define VC0706_READ_DATA 0x30
+#define VC0706_WRITE_DATA 0x31
+#define VC0706_COMM_MOTION_CTRL 0x37
+#define VC0706_COMM_MOTION_STATUS 0x38
+#define VC0706_COMM_MOTION_DETECTED 0x39
+#define VC0706_MOTION_CTRL 0x42
+#define VC0706_MOTION_STATUS 0x43
+#define VC0706_TVOUT_CTRL 0x44
+#define VC0706_OSD_ADD_CHAR 0x45
+
+#define VC0706_STOPCURRENTFRAME 0x0
+#define VC0706_STOPNEXTFRAME 0x1
+#define VC0706_RESUMEFRAME 0x3
+#define VC0706_STEPFRAME 0x2
+
+#define VC0706_640x480 0x00
+#define VC0706_320x240 0x11
+#define VC0706_160x120 0x22
+
+#define VC0706_MOTIONCONTROL 0x0
+#define VC0706_UARTMOTION 0x01
+#define VC0706_ACTIVATEMOTION 0x01
+
+#define VC0706_SET_ZOOM 0x52
+#define VC0706_GET_ZOOM 0x53
+
+//#define CAMERABUFFSIZ 100
+#define CAMERADELAY 10
+
+
+
+
+/// \brief Camera class
+class Camera{
+    static Serial* camera;
+    static uint8_t  serialNum;
+    uint8_t bufferLen;
+    uint8_t  camerabuff[CAMERABUFFSIZ+1];
+    uint16_t frameptr;
+public:
+    /// \brief Call this function to get a hold of the serial for camera.
+    static Serial& getSerial();
+    /// \brief Reset the camera
+    bool reset();
+    /// \brief Get the camera version
+    char * getVersion(void);
+    
+    bool takePicture();
+    bool cameraFrameBuffCtrl(uint8_t command);
+    uint8_t* readPicture(uint8_t n,uint8_t* outSize=NULL);
+    uint32_t frameLength(void);
+    /// \brief Get the image Size
+    uint8_t getImageSize();
+    /// \brief Set the image Size
+    bool setImageSize(uint8_t x);
+    //
+    // Low level commands
+    //
+    // Run a command
+    bool runCommand(uint8_t cmd, uint8_t *args, uint8_t argn, uint8_t resplen, bool flushflag = true);
+    // Output the buffer over the USB
+    void printBuff();
+    // Varify the response
+    bool verifyResponse(uint8_t command);
+    // Read the response
+    uint8_t readResponse(uint8_t numbytes, uint8_t timeout);
+    // Send the command
+    void sendCommand(uint8_t cmd, uint8_t args[] = 0, uint8_t argn = 0);
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/gps.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,12 @@
+#include "gps.h"
+
+Serial* GPS::gps=NULL;
+
+Serial& GPS::getSerial(){
+    if(gps==NULL){
+        // Init Serial USB
+        gps=new Serial(GPSPINTX,GPSPINRX);
+        gps->baud(GPSBAUD);//gps->baud(115200);//57600);
+    }
+    return *pc;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/gps.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,20 @@
+#ifndef _GPS_H_
+#define _GPS_H_
+#include "mbed.h"
+
+
+/// Define Pinout
+#define GPSPINTX p9
+#define GPSPINRX p10
+
+/// Define Baud
+#define GPSBAUD 57600
+
+class GPS{
+private:
+    static Serial* gps;
+public:
+    /// \brief Call this function to get a hold of the serial for USB.
+    static Serial& getSerial();
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/usb.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,13 @@
+
+#include "usb.h"
+
+Serial* USB::pc=NULL;
+
+Serial& USB::getSerial(){
+    if(pc==NULL){
+        // Init Serial USB
+        pc=new Serial(USBTX,USBRX);
+        pc->baud(115200);//57600);
+    }
+    return *pc;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/usb.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,11 @@
+#ifndef _USB_H_
+#define _USB_H_
+#include "mbed.h"
+class USB{
+private:
+    static Serial* pc;
+public:
+    /// \brief Call this function to get a hold of the serial for USB.
+    static Serial& getSerial();
+};
+#endif
\ No newline at end of file

--- a/camera.cpp	Fri Feb 21 15:15:25 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,372 +0,0 @@
-/*************************************************** 
-  This is a library for the Adafruit TTL JPEG Camera (VC0706 chipset)
-
-  Pick one up today in the adafruit shop!
-  ------> http://www.adafruit.com/products/397
-
-  These displays use Serial to communicate, 2 pins are required to interface
-
-  Adafruit invests time and resources providing this open source code, 
-  please support Adafruit and open-source hardware by purchasing 
-  products from Adafruit!
-
-  Written by Limor Fried/Ladyada for Adafruit Industries.  
-  BSD license, all text above must be included in any redistribution
- ****************************************************/
-
-
-#include "camera.h"
-
-Serial* Camera::camera=NULL;
-uint8_t Camera::serialNum=0;
-
-Serial& Camera::getSerial(){
-    if(camera==NULL){
-        // Init Camera
-        camera=new Serial(CAMERAPINTX,CAMERAPINRX);
-        camera->baud(CAMERABAUD);
-        serialNum=0;
-        Camera temp;
-        temp.reset();
-    }
-    return *camera;
-}
-
-bool Camera::reset() {
-  uint8_t args[] = {0x0};
-  return runCommand(VC0706_RESET, args, 1, 5);
-}
-
-/*
-// Initialization code used by all constructor types
-void Camera::common_init(void) {
-  swSerial  = NULL;
-  hwSerial  = NULL;
-  frameptr  = 0;
-  bufferLen = 0;
-  serialNum = 0;
-}
-
-// Constructor when using SoftwareSerial or NewSoftSerial
-#if ARDUINO >= 100
-Camera::Camera(SoftwareSerial *ser) {
-#else
-Camera::Camera(NewSoftSerial *ser) {
-#endif
-  common_init();  // Set everything to common state, then...
-  swSerial = ser; // ...override swSerial with value passed.
-}
-
-// Constructor when using HardwareSerial
-Camera::Camera(HardwareSerial *ser) {
-  common_init();  // Set everything to common state, then...
-  hwSerial = ser; // ...override hwSerial with value passed.
-}
-
-boolean Camera::begin(uint16_t baud) {
-  if(swSerial) swSerial->begin(baud);
-  else         hwSerial->begin(baud);
-  return reset();
-}
-
-
-boolean Camera::motionDetected() {
-  if (readResponse(4, 200) != 4) {
-    return false;
-  }
-  if (! verifyResponse(VC0706_COMM_MOTION_DETECTED))
-    return false;
-
-  return true;
-}
-
-
-uint8_t Camera::setMotionStatus(uint8_t x, uint8_t d1, uint8_t d2) {
-  uint8_t args[] = {0x03, x, d1, d2};
-
-  return runCommand(VC0706_MOTION_CTRL, args, sizeof(args), 5);
-}
-
-
-uint8_t Camera::getMotionStatus(uint8_t x) {
-  uint8_t args[] = {0x01, x};
-
-  return runCommand(VC0706_MOTION_STATUS, args, sizeof(args), 5);
-}
-
-
-boolean Camera::setMotionDetect(boolean flag) {
-  if (! setMotionStatus(VC0706_MOTIONCONTROL, 
-            VC0706_UARTMOTION, VC0706_ACTIVATEMOTION))
-    return false;
-
-  uint8_t args[] = {0x01, flag};
-  
-  runCommand(VC0706_COMM_MOTION_CTRL, args, sizeof(args), 5);
-}
-
-
-
-boolean Camera::getMotionDetect(void) {
-  uint8_t args[] = {0x0};
-
-  if (! runCommand(VC0706_COMM_MOTION_STATUS, args, 1, 6))
-    return false;
-
-  return camerabuff[5];
-}
-*/
-uint8_t Camera::getImageSize() {
-  uint8_t args[] = {0x4, 0x4, 0x1, 0x00, 0x19};
-  if (! runCommand(VC0706_READ_DATA, args, sizeof(args), 6))
-    return 0xFF;
-
-  return camerabuff[5];
-}
-
-bool Camera::setImageSize(uint8_t x) {
-  uint8_t args[] = {0x05, 0x04, 0x01, 0x00, 0x19, x};
-  return runCommand(VC0706_WRITE_DATA, args, sizeof(args), 5);
-}
-/*
-// downsize image control 
-
-uint8_t Camera::getDownsize(void) {
-  uint8_t args[] = {0x0};
-  if (! runCommand(VC0706_DOWNSIZE_STATUS, args, 1, 6)) 
-    return -1;
-
-   return camerabuff[5];
-}
-
-boolean Camera::setDownsize(uint8_t newsize) {
-  uint8_t args[] = {0x01, newsize};
-
-  return runCommand(VC0706_DOWNSIZE_CTRL, args, 2, 5);
-}
-
-// other high level commands 
-*/
-char * Camera::getVersion(void) {
-  uint8_t args[] = {0x01};
-  
-  sendCommand(VC0706_GEN_VERSION, args, 1);
-  // get reply
-  if (!readResponse(CAMERABUFFSIZ, 200)){
-    return 0;
-  }
-  camerabuff[bufferLen] = 0;  // end it!
-  return (char *)camerabuff;  // return it!
-}
-/*
-
-// high level photo comamnds
-
-void Camera::OSD(uint8_t x, uint8_t y, char *str) {
-  if (strlen(str) > 14) { str[13] = 0; }
-
-  uint8_t args[17] = {strlen(str), strlen(str)-1, (y & 0xF) | ((x & 0x3) << 4)};
-
-  for (uint8_t i=0; i<strlen(str); i++) {
-    char c = str[i];
-    if ((c >= '0') && (c <= '9')) {
-      str[i] -= '0';
-    } else if ((c >= 'A') && (c <= 'Z')) {
-      str[i] -= 'A';
-      str[i] += 10;
-    } else if ((c >= 'a') && (c <= 'z')) {
-      str[i] -= 'a';
-      str[i] += 36;
-    }
-
-    args[3+i] = str[i];
-  }
-
-   runCommand(VC0706_OSD_ADD_CHAR, args, strlen(str)+3, 5);
-   printBuff();
-}
-
-boolean Camera::setCompression(uint8_t c) {
-  uint8_t args[] = {0x5, 0x1, 0x1, 0x12, 0x04, c};
-  return runCommand(VC0706_WRITE_DATA, args, sizeof(args), 5);
-}
-
-uint8_t Camera::getCompression(void) {
-  uint8_t args[] = {0x4, 0x1, 0x1, 0x12, 0x04};
-  runCommand(VC0706_READ_DATA, args, sizeof(args), 6);
-  printBuff();
-  return camerabuff[5];
-}
-
-boolean Camera::setPTZ(uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) {
-  uint8_t args[] = {0x08, wz >> 8, wz, 
-            hz >> 8, wz, 
-            pan>>8, pan, 
-            tilt>>8, tilt};
-
-  return (! runCommand(VC0706_SET_ZOOM, args, sizeof(args), 5));
-}
-
-
-boolean Camera::getPTZ(uint16_t &w, uint16_t &h, uint16_t &wz, uint16_t &hz, uint16_t &pan, uint16_t &tilt) {
-  uint8_t args[] = {0x0};
-  
-  if (! runCommand(VC0706_GET_ZOOM, args, sizeof(args), 16))
-    return false;
-  printBuff();
-
-  w = camerabuff[5];
-  w <<= 8;
-  w |= camerabuff[6];
-
-  h = camerabuff[7];
-  h <<= 8;
-  h |= camerabuff[8];
-
-  wz = camerabuff[9];
-  wz <<= 8;
-  wz |= camerabuff[10];
-
-  hz = camerabuff[11];
-  hz <<= 8;
-  hz |= camerabuff[12];
-
-  pan = camerabuff[13];
-  pan <<= 8;
-  pan |= camerabuff[14];
-
-  tilt = camerabuff[15];
-  tilt <<= 8;
-  tilt |= camerabuff[16];
-
-  return true;
-}
-
-*/
-bool Camera::takePicture() {
-  frameptr = 0;
-  return cameraFrameBuffCtrl(VC0706_STOPCURRENTFRAME); 
-}
-/*
-boolean Camera::resumeVideo() {
-  return cameraFrameBuffCtrl(VC0706_RESUMEFRAME); 
-}
-
-boolean Camera::TVon() {
-  uint8_t args[] = {0x1, 0x1};
-  return runCommand(VC0706_TVOUT_CTRL, args, sizeof(args), 5);
-}
-boolean Camera::TVoff() {
-  uint8_t args[] = {0x1, 0x0};
-  return runCommand(VC0706_TVOUT_CTRL, args, sizeof(args), 5);
-}
-*/
-bool Camera::cameraFrameBuffCtrl(uint8_t command) {
-  uint8_t args[] = {0x1, command};
-  return runCommand(VC0706_FBUF_CTRL, args, sizeof(args), 5);
-}
-
-uint32_t Camera::frameLength(void) {
-  uint8_t args[] = {0x01, 0x00};
-  if (!runCommand(VC0706_GET_FBUF_LEN, args, sizeof(args), 9))
-    return 0;
-
-  uint32_t len;
-  len = camerabuff[5];
-  len <<= 8;
-  len |= camerabuff[6];
-  len <<= 8;
-  len |= camerabuff[7];
-  len <<= 8;
-  len |= camerabuff[8];
-
-  return len;
-}
-/*
-
-uint8_t Camera::available(void) {
-  return bufferLen;
-}
-
-*/
-uint8_t * Camera::readPicture(uint8_t n,uint8_t* outSize) {
-  uint8_t args[] = {0x0C, 0x0, 0x0A, 
-                    0, 0, frameptr >> 8, frameptr & 0xFF, 
-                    0, 0, 0, n, 
-                    CAMERADELAY >> 8, CAMERADELAY & 0xFF};
-
-  if (! runCommand(VC0706_READ_FBUF, args, sizeof(args), 5, false))
-    return 0;
-
-
-  // read into the buffer PACKETLEN!
-  int outputSize=readResponse(n+5, CAMERADELAY)-5;
-  if (outputSize == 0) 
-      return 0;
-  if(outSize!=NULL)*outSize=outputSize;
-
-  frameptr += outputSize;
-
-  return camerabuff;
-}
-
-
-//
-// Low level commands
-//
-
-bool Camera::runCommand(uint8_t cmd, uint8_t *args, uint8_t argn, uint8_t resplen, bool flushflag) {
-  // flush out anything in the buffer?
-  if (flushflag) {
-    readResponse(100, 10); 
-  }
-
-  sendCommand(cmd, args, argn);
-  if (readResponse(resplen, 200) != resplen) 
-    return false;
-  if (! verifyResponse(cmd))
-    return false;
-  return true;
-}
-
-
-void Camera::sendCommand(uint8_t cmd, uint8_t args[], uint8_t argn) {
-    getSerial().putc(0x56);
-    getSerial().putc(serialNum);
-    getSerial().putc(cmd);
-    
-    for (uint8_t i=0; i<argn; i++) {
-      getSerial().putc(args[i]);
-    }
-}
-
-uint8_t Camera::readResponse(uint8_t numbytes, uint8_t timeout) {
-  uint8_t counter = 0;
-  bufferLen = 0;
- 
-  while ((timeout != counter) && (bufferLen != numbytes)){
-    int avail = getSerial().readable();
-    if (avail <= 0) {
-      wait_ms(1);
-      counter++;
-      continue;
-    }
-    counter = 0;
-    // there's a byte!
-    camerabuff[bufferLen++] = getSerial().getc();
-  }
-  return bufferLen;
-}
-
-bool Camera::verifyResponse(uint8_t command) {
-  if ((camerabuff[0] != 0x76) ||
-      (camerabuff[1] != serialNum) ||
-      (camerabuff[2] != command) ||
-      (camerabuff[3] != 0x0))
-      return false;
-  return true;
-  
-}
-
-void Camera::printBuff() {
-}
\ No newline at end of file

--- a/camera.h	Fri Feb 21 15:15:25 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,92 +0,0 @@
-/**
-  *  \brief Camera interface
-  **/
-#define CAMERABUFFSIZ 100
-#include "usb.h"
-#include "mbed.h"
-
-/// Define Pinout
-#define CAMERAPINTX p9
-//p13
-#define CAMERAPINRX p10
-//p14
-/// Define Baud
-#define CAMERABAUD 38400
-
-/// Define commands
-#define VC0706_RESET  0x26
-#define VC0706_GEN_VERSION 0x11
-#define VC0706_READ_FBUF 0x32
-#define VC0706_GET_FBUF_LEN 0x34
-#define VC0706_FBUF_CTRL 0x36
-#define VC0706_DOWNSIZE_CTRL 0x54
-#define VC0706_DOWNSIZE_STATUS 0x55
-#define VC0706_READ_DATA 0x30
-#define VC0706_WRITE_DATA 0x31
-#define VC0706_COMM_MOTION_CTRL 0x37
-#define VC0706_COMM_MOTION_STATUS 0x38
-#define VC0706_COMM_MOTION_DETECTED 0x39
-#define VC0706_MOTION_CTRL 0x42
-#define VC0706_MOTION_STATUS 0x43
-#define VC0706_TVOUT_CTRL 0x44
-#define VC0706_OSD_ADD_CHAR 0x45
-
-#define VC0706_STOPCURRENTFRAME 0x0
-#define VC0706_STOPNEXTFRAME 0x1
-#define VC0706_RESUMEFRAME 0x3
-#define VC0706_STEPFRAME 0x2
-
-#define VC0706_640x480 0x00
-#define VC0706_320x240 0x11
-#define VC0706_160x120 0x22
-
-#define VC0706_MOTIONCONTROL 0x0
-#define VC0706_UARTMOTION 0x01
-#define VC0706_ACTIVATEMOTION 0x01
-
-#define VC0706_SET_ZOOM 0x52
-#define VC0706_GET_ZOOM 0x53
-
-#define CAMERABUFFSIZ 100
-#define CAMERADELAY 10
-
-
-
-
-/// \brief Camera class
-class Camera{
-    static Serial* camera;
-    static uint8_t  serialNum;
-    uint8_t bufferLen;
-    uint8_t  camerabuff[CAMERABUFFSIZ+1];
-    uint16_t frameptr;
-public:
-    /// \brief Call this function to get a hold of the serial for camera.
-    static Serial& getSerial();
-    /// \brief Reset the camera
-    bool reset();
-    /// \brief Get the camera version
-    char * getVersion(void);
-    
-    bool takePicture();
-    bool cameraFrameBuffCtrl(uint8_t command);
-    uint8_t* readPicture(uint8_t n,uint8_t* outSize=NULL);
-    uint32_t frameLength(void);
-    /// \brief Get the image Size
-    uint8_t getImageSize();
-    /// \brief Set the image Size
-    bool setImageSize(uint8_t x);
-    //
-    // Low level commands
-    //
-    // Run a command
-    bool runCommand(uint8_t cmd, uint8_t *args, uint8_t argn, uint8_t resplen, bool flushflag = true);
-    // Output the buffer over the USB
-    void printBuff();
-    // Varify the response
-    bool verifyResponse(uint8_t command);
-    // Read the response
-    uint8_t readResponse(uint8_t numbytes, uint8_t timeout);
-    // Send the command
-    void sendCommand(uint8_t cmd, uint8_t args[] = 0, uint8_t argn = 0);
-};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/dataLocation.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,34 @@
+
+
+DataLocation* LocHolder::locs=NULL;
+DataLocation* LocHolder::targ=NULL;
+DataLocation* LocHolder::base=NULL;
+
+DataLocation* LocHolder::getLocs(){
+    if(locs==NULL)locs=new DataLocation[MAXNUMLOCS];
+    return locs;
+}
+
+DataLocation* LocHolder::getTarg(){
+    if(targ==NULL)targ=new DataLocation[MAXNUMLOCS];
+    return targ;
+}
+
+DataLocation* LocHolder::getBase(){
+    if(base==NULL)base=new DataLocation[MAXNUMLOCS];
+    return base;
+}
+
+unsigned int LocHolder::getRealIndex(int index,int offset=0){
+    return (index+offset)%MAXNUMLOCS;
+}
+
+DataLocation& LocHolder::getCurrentLocs(int offset=0){
+    return getLocs()[getRealIndex(headLocs,offset)];
+}
+DataLocation& LocHolder::getCurrentTarg(int offset=0){
+    return getTarg()[getRealIndex(headTarg,offset)];
+}
+DataLocation& LocHolder::getCurrentBase(int offset=0){
+    return getBase()[getRealIndex(headBase,offset)];
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/dataLocation.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,35 @@
+#define MAXNUMLOCS 256
+
+// Storage of data location
+class DataLocation{
+private:
+    // Current value of lat lon and alt
+    double lat,lon,alt;
+public:
+    double& getLat(){return lat;}
+    double& getLon(){return lon;}
+    double& getAlt(){return alt;}
+}
+
+// Singleton location holder
+class LocHolder{
+private:
+    // Actual Locations (absolute)
+    static DataLocation locs[MAXNUMLOCS];
+    // Target Locations (relative to base station -> base station is at 0,0,0)
+    static DataLocation targ[MAXNUMLOCS];
+    // Base Station Locations (absolute)
+    static DataLocation base[MAXNUMLOCS];
+    // Index of the head of the circular buffers
+    static unsigned int headLocs,headTarg,headBase;
+public:
+    static DataLocation* getLocs();
+    static DataLocation* getTarg();
+    static DataLocation* getBase();
+    
+    DataLocation& getCurrentLocs(int offset=0);
+    DataLocation& getCurrentTarg(int offset=0);
+    DataLocation& getCurrentBase(int offset=0);
+    
+    
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleCamera.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,41 @@
+#include "takeImage.h"
+
+void ImageHandle::setup(){
+    //char* version = cam.getVersion();
+    uint8_t targetSize=VC0706_160x120;//VC0706_640x480;//VC0706_160x120;
+    cam.setImageSize(targetSize);
+    uint8_t realSize=cam.getImageSize();
+}
+
+void ImageHandle::take(){
+    if (! cam.takePicture()) {
+        USB::getSerial().printf("Failed to snap!\n");
+        while(1){}
+    }
+    int size=cam.frameLength();
+    USB::getSerial().printf("Image Start\n",size);
+    int i;
+    for(i=0;i<size;){
+        // read 32 bytes at a time;
+        uint8_t bytesToRead = min(64, size-i); // change 32 to 64 for a speedup but may not work with all setups!
+        uint8_t bytesRead=0;
+        uint8_t* buffer = cam.readPicture(bytesToRead,&bytesRead);
+        for(int a=0;a<bytesRead;a++){USB::getSerial().putc(buffer[a]);}
+        i+=bytesRead;
+    }
+    USB::getSerial().printf("Image End\n",size);
+}
+
+bool ImageHandle::check(){
+    return true;
+}
+
+void ImageHandle::run(){
+    if(!initialized){
+        initialized=true;
+        setup();
+    }
+    if(check()){
+        take();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleCamera.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,24 @@
+#ifndef _TAKEIMAGE_H_
+#define _TAKEIMAGE_H_
+
+#include "adapt/usb.h"
+#include "adapt/camera.h"
+class ImageHandle{
+private:
+    Camera cam;
+    bool initialized;
+    
+    /// \brief Setup the camera.
+    void setup();
+    /// \brief Take an image and send it over USB
+    void take();
+    /// \brief Check if an image must be taken
+    bool check();
+public:
+    /// \brief Constructor
+    ImageHandle():initialized(false){}
+    /// \brief Run an instance of this
+    void run();
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleGPS.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,15 @@
+void GPSHandle::setup(){
+}
+
+void GPSHandle::update(){
+    while(true){
+        if(gps.readable()>0){
+            char c = gps.getc();
+            USB::getSerial().printf("%c",c);
+        }
+    }
+}
+
+bool GPSHandle::check(){
+    return true;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleGPS.h	Tue Apr 01 15:52:08 2014 +0000
@@ -0,0 +1,24 @@
+#ifndef _HANDLEGPS_H_
+#define _HANDLEGPS_H_
+
+#include "adapt/usb.h"
+#include "adapt/camera.h"
+class GPSHandle{
+private:
+    GPS gps
+    bool initialized;
+    
+    /// \brief Setup
+    void setup();
+    /// \brief Update the GPS locations
+    void update();
+    /// \brief Check if it is time to update
+    bool check();
+public:
+    /// \brief Constructor
+    ImageHandle():initialized(false){}
+    /// \brief Run an instance of this
+    void run();
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Fri Feb 21 15:15:25 2014 +0000
+++ b/main.cpp	Tue Apr 01 15:52:08 2014 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include <string>
 #include <sstream>
-#include "usb.h"
-#include "camera.h"
+#include "adapt/usb.h"
+#include "adapt/camera.h"
 /*
 Serial pc(USBTX,USBRX);
 Serial xbee(p9,p10);//tx, rx
@@ -150,34 +150,30 @@
         reading = true;
      }
 }
+
+
 */
+Serial gps(p28,p27);
+
 int main(){
-    //USB::getSerial().printf("------Starting Program------\n");
-    Camera cam;
-    char* version = cam.getVersion();
-    //USB::getSerial().printf("Camera Version:\n%s\n",version);
-    uint8_t targetSize=VC0706_640x480;//VC0706_160x120;
-    cam.setImageSize(targetSize);
-    uint8_t realSize=cam.getImageSize();
-    //USB::getSerial().printf("Image Size | Target: %d, Real %d",targetSize,realSize);
-    if (! cam.takePicture()) {
-        USB::getSerial().printf("Failed to snap!\n");
-        while(1){}
-    } else {
-        //USB::getSerial().printf("Picture taken!\n");
+    ImageHandle imageHand;
+    GPSHand gpsHand;
+    
+    /// Main Loop
+    while(1){
+        // Run image handler
+        imageHand.run();
+        // Run GPS handler
+        gpsHand.run();
     }
-    int size=cam.frameLength();
-    //USB::getSerial().printf("Image Size %d\n",size);
-    int i;
-    for(i=0;i<size;){
-        // read 32 bytes at a time;
-        uint8_t bytesToRead = min(64, size-i); // change 32 to 64 for a speedup but may not work with all setups!
-        uint8_t bytesRead=0;
-        uint8_t* buffer = cam.readPicture(bytesToRead,&bytesRead);
-        for(int a=0;a<bytesRead;a++){USB::getSerial().putc(buffer[a]);}
-        //USB::getSerial().write(buffer,bytesToRead);
-        //USB::getSerial().printf("Read: %d/%d - %d/%d\n",i,size,bytesRead,bytesToRead);
-        i+=bytesRead;
+    
+    USB::getSerial().printf("Start!\n");
+    gps.baud(57600);
+    while(true){
+        if(gps.readable()>0){
+            char c = gps.getc();
+            USB::getSerial().printf("%c",c);
+        }
     }
     //USB::getSerial().printf("Done(%d)\n",i);
     while(1){

--- a/usb.cpp	Fri Feb 21 15:15:25 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-
-#include "usb.h"
-
-Serial* USB::pc=NULL;
-
-Serial& USB::getSerial(){
-    if(pc==NULL){
-        // Init Serial USB
-        pc=new Serial(USBTX,USBRX);
-        pc->baud(115200);//57600);
-    }
-    return *pc;
-}
\ No newline at end of file

--- a/usb.h	Fri Feb 21 15:15:25 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,11 +0,0 @@
-#ifndef _USB_H_
-#define _USB_H_
-#include "mbed.h"
-class USB{
-private:
-    static Serial* pc;
-public:
-    /// \brief Call this function to get a hold of the serial for USB.
-    static Serial& getSerial();
-};
-#endif
\ No newline at end of file