Updated PID values with Ziegler/Nichols

Got frames from pixy to emulator. Slooooow.

.cproject

@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.cross.base.1368576398">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.1368576398" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="${ProjName}" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.cross.base.1368576398" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.1368576398.357960121" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.cross.base.1769959713" name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.base">
-							<option id="cdt.managedbuild.option.gnu.cross.prefix.293575246" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"/>
-							<option id="cdt.managedbuild.option.gnu.cross.path.782624596" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"/>
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.174263624" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
-							<builder id="cdt.managedbuild.builder.gnu.cross.1279514893" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.builder.gnu.cross"/>
-							<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.1136001261" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
-								<option id="gnu.c.compiler.option.include.paths.649992569" superClass="gnu.c.compiler.option.include.paths" valueType="includePath">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/example/libs/BSP}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/example/libs/sGUI}&quot;"/>
-									<listOptionValue builtIn="false" value="/usr/arm-none-eabi/include"/>
-									<listOptionValue builtIn="false" value="/usr/lib/gcc/arm-none-eabi/4.9.2/include"/>
-								</option>
-								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1184220438" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1669992748" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
-								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.564868280" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1771545603" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
-							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.490606250" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
-								<option id="gnu.cpp.link.option.paths.312574565" superClass="gnu.cpp.link.option.paths" valueType="libPaths">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/example/libs}&quot;"/>
-								</option>
-								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.648346019" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
-									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
-									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
-								</inputType>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1024233109" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
-							<tool id="cdt.managedbuild.tool.gnu.cross.assembler.1636598755" name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
-								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1738279002" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
-							</tool>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="example.null.999619443" name="example"/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-	<storageModule moduleId="refreshScope" versionNumber="2">
-		<configuration configurationName="Default">
-			<resource resourceType="PROJECT" workspacePath="/example"/>
-		</configuration>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1368576398;cdt.managedbuild.toolchain.gnu.cross.base.1368576398.357960121;cdt.managedbuild.tool.gnu.cross.c.compiler.1136001261;cdt.managedbuild.tool.gnu.c.compiler.input.1184220438">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1368576398;cdt.managedbuild.toolchain.gnu.cross.base.1368576398.357960121;cdt.managedbuild.tool.gnu.cross.cpp.compiler.1669992748;cdt.managedbuild.tool.gnu.cpp.compiler.input.564868280">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
-</cproject>

README.md

@@ -1,6 +1,15 @@
-Discoverpixy
+# discoverpixy
-============================
+Project with the Pixy cam and the STM32F4 Discovery
 
-#TODO
+## Folder structure
-Adjust this readme
+* *common*: device independent code and the "Application" itself
+* *emulator*: display/touch/sd emulator written in qt. pixy can be connect via usb to computer
+* *discovery*: display/touch/sd/pixy can be connected to STM32F4Discovery
 
+All code in the common folder **MUST** be device independent. 
+
+The folder *common/lowlevel* provides the function headers for the device specific code.
+
+
+## How to build
+Go into the *emulator* or the *discovery* folder an use *make*

common/.gitignore

@@ -0,0 +1,2 @@
+# Temporary files
+*.swp

common/app/app.c

@@ -0,0 +1,24 @@
+#include "app.h"
+#include "tft.h"
+#include "system.h"
+#include "touch.h"
+#include "screen_main.h"
+#include "filesystem.h"
+
+void app_init() {
+	system_init();
+	tft_init();
+	touch_init();
+	filesystem_init();
+
+	gui_screen_navigate(get_screen_main());
+}
+
+//app event loop
+void app_process() {
+	
+	system_process(); //Let the system handle it's pending events
+	gui_screen_update(); //update the currently active screen
+}
+
+

common/app/app.h

@@ -0,0 +1,25 @@
+#ifndef APP_H
+#define APP_H
+
+/**
+ * @defgroup app Application
+ * The App Module contains the effective, platform independent application.
+ */
+/*@{*/
+
+/**
+ * Starts/Initializes the app
+ * This function should be called at the top of the main function of your platform
+ */
+void app_init();
+
+/**
+ * Executes one cycle of the app
+ * Call this function repeatedly from a loop inside the main function
+ */
+void app_process();
+
+
+/*@}*/
+
+#endif /* APP_H */

common/app/pixy_control.c

@@ -0,0 +1,73 @@
+/*
+ * pixy_control.c
+ *
+ * Notation
+ * --------
+ *
+ * x    :   Sollwert (Führgrösse)
+ * w    :   Istwert (Reglergrösse)
+ * esum :   Integralteil
+ * e    :   Regelabweichung
+ * y    :   Stellgrösse
+ *
+ *
+ */
+#include<pixy_control.h>
+#include<stdint.h>
+
+// PID tuning factors
+#define REG_PID_KP      (0.41f)
+#define REG_PID_KI      (0.001f)
+#define REG_PID_KD      (0.00025f)
+#define REG_PID_TA      (0.001f)
+#define REG_PID_YKOR    (0.3f)
+
+void int_init(void){  
+    // TODO Init ports and outputs if needed.
+}
+
+// PID controller implementatoin for the y-axis
+int16_t pixy_PID_Y(int16_t x, int16_t w)
+{
+    float e = 0;
+    static float esum = 0;
+    static float eold = 0;
+    float y = 0;
+
+    e = (float)(x - w);                         // calculate the controller offset
+
+    //----PID-control-------------------------------------------------------------------------
+    esum = esum + e;                            // add e to the current sum
+
+    y += (REG_PID_KP + REG_PID_YKOR) * e;       // add the proportional part to the output
+    y += REG_PID_KI * REG_PID_TA * esum;        // add the integral part to the output
+    y += REG_PID_KD * (e - eold) / REG_PID_TA;  // add the differential part to the output
+    //----------------------------------------------------------------------------------------
+
+    eold = e;                                   // save the previous value
+
+    return (int16_t)y;
+}
+
+// PID controller implementation for the x-axis
+int16_t pixy_PID_X(int16_t x, int16_t w)
+{
+    float e = 0;
+    static float esum = 0;
+    static float eold = 0;
+    float y = 0;
+
+    e = (float)(x - w);                         // calculate the controller offset
+
+    //----PID-control-------------------------------------------------------------------------
+    esum = esum + e;                            // add e to the current sum
+
+    y += REG_PID_KP * e;                        // add the proportional part to the output
+    y += REG_PID_KI * REG_PID_TA * esum;        // add the integral part to the output
+    y += REG_PID_KD * (e - eold) / REG_PID_TA;  // add the differential part to the output
+    //----------------------------------------------------------------------------------------
+
+    eold = e;                                   // save the previous value
+
+    return (int16_t)y;
+}

common/app/pixy_control.h

@@ -0,0 +1,14 @@
+/* 
+ * pixy_control.h
+ */
+
+#ifndef _CONTROL_H_
+#define _CONTROL_H_
+
+#include<stdint.h>
+
+void int_init(void);
+int16_t pixy_PID_Y(int16_t x, int16_t w);
+int16_t pixy_PID_X(int16_t x, int16_t w);
+
+#endif

common/app/pixy_helper.c

@@ -0,0 +1,241 @@
+#include "pixy_helper.h"
+#include "pixy.h"
+#include "tft.h"
+#include <stdlib.h>
+
+
+
+static int renderBA81(uint16_t xpos, uint16_t ypos, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame);
+static int saveBA81(FILE_HANDLE* handle, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame);
+
+
+int pixy_render_full_frame(uint16_t x, uint16_t y) {
+	return pixy_render_cropped_frame(x,y,0,0,320,200);
+}
+
+
+int pixy_render_cropped_frame(uint16_t x, uint16_t y, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height) {
+	uint8_t* videodata;
+	int32_t response;
+	int32_t fourccc;
+	int8_t renderflags;
+	uint16_t xwidth;
+	uint16_t ywidth;
+	uint32_t size;
+
+
+	int return_value = pixy_command("cam_getFrame",  // String id for remote procedure
+								  INT8(0x21),     // mode
+								  INT16(xoffset),        // xoffset
+								  INT16(yoffset),         // yoffset
+								  INT16(width),       // width
+								  INT16(height),       // height
+								  END_OUT_ARGS,              // separator
+								  &response,      // pointer to mem address for return value
+								  &fourccc,
+								  &renderflags,
+								  &xwidth,
+								  &ywidth,
+								  &size,
+								  &videodata,        // pointer to mem address for returned frame
+								  END_IN_ARGS);
+
+	if(return_value==0) {
+		return_value = renderBA81(x,y,xwidth,ywidth,size,videodata);
+	}
+
+	return return_value;
+}
+
+int pixy_save_full_frame(FILE_HANDLE* handle) {
+	return pixy_save_cropped_frame(handle,0,0,320,200);
+}
+
+int pixy_save_cropped_frame(FILE_HANDLE* handle, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height) {
+	uint8_t* videodata;
+	int32_t response;
+	int32_t fourccc;
+	int8_t renderflags;
+	uint16_t xwidth;
+	uint16_t ywidth;
+	uint32_t size;
+
+
+	int return_value = pixy_command("cam_getFrame",  // String id for remote procedure
+								  INT8(0x21),     // mode
+								  INT16(xoffset),        // xoffset
+								  INT16(yoffset),         // yoffset
+								  INT16(width),       // width
+								  INT16(height),       // height
+								  END_OUT_ARGS,              // separator
+								  &response,      // pointer to mem address for return value
+								  &fourccc,
+								  &renderflags,
+								  &xwidth,
+								  &ywidth,
+								  &size,
+								  &videodata,        // pointer to mem address for returned frame
+								  END_IN_ARGS);
+
+	if(return_value==0) {
+		return_value = saveBA81(handle,xwidth,ywidth,size,videodata);
+	}
+
+	return return_value;
+}
+
+
+
+
+
+static void interpolateBayer(uint16_t width, uint16_t x, uint16_t y, uint8_t *pixel, uint8_t* r, uint8_t* g, uint8_t* b)
+{
+    if (y&1)
+    {
+        if (x&1)
+        {
+            *r = *pixel;
+            *g = (*(pixel-1)+*(pixel+1)+*(pixel+width)+*(pixel-width))>>2;
+            *b = (*(pixel-width-1)+*(pixel-width+1)+*(pixel+width-1)+*(pixel+width+1))>>2;
+        }
+        else
+        {
+            *r = (*(pixel-1)+*(pixel+1))>>1;
+            *g = *pixel;
+            *b = (*(pixel-width)+*(pixel+width))>>1;
+        }
+    }
+    else
+    {
+        if (x&1)
+        {
+            *r = (*(pixel-width)+*(pixel+width))>>1;
+            *g = *pixel;
+            *b = (*(pixel-1)+*(pixel+1))>>1;
+        }
+        else
+        {
+            *r = (*(pixel-width-1)+*(pixel-width+1)+*(pixel+width-1)+*(pixel+width+1))>>2;
+            *g = (*(pixel-1)+*(pixel+1)+*(pixel+width)+*(pixel-width))>>2;
+            *b = *pixel;
+        }
+    }
+
+}
+
+
+
+
+
+
+static int renderBA81(uint16_t xpos, uint16_t ypos, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame)
+{
+    uint16_t x, y;
+    uint8_t r, g, b;
+
+
+    // skip first line
+    frame += width;
+
+    // don't render top and bottom rows, and left and rightmost columns because of color
+    // interpolation
+    //uint32_t decodedimage[(width-2)*(height-2)];
+    uint16_t* decodedimage = malloc(sizeof(uint16_t)*(width-2)*(height-2));
+
+    if(decodedimage==NULL) { //not enough free space to decode image in memory
+        //decode & render image pixel by pixel
+        for (y=1; y<height-1; y++)
+        {
+                frame++;
+                for (x=1; x<width-1; x++, frame++)
+                {
+                    interpolateBayer(width, x, y, frame, &r, &g, &b);
+                    tft_draw_pixel(xpos+x-1,ypos+y-1,RGB(r,g,b));
+                }
+                frame++;
+         }
+    } else { //enough space
+            uint16_t* line = decodedimage;
+            for (y=1; y<height-1; y++)
+            {
+                //line = (unsigned int *)img.scanLine(y-1);
+                frame++;
+                for (x=1; x<width-1; x++, frame++)
+                {
+                    interpolateBayer(width, x, y, frame, &r, &g, &b);
+                    //*line++ = (0xff<<24) | (r<<16) | (g<<8) | (b<<0);
+                    *line++ = RGB(r,g,b);
+                }
+                frame++;
+            }
+
+            tft_draw_bitmap_unscaled(xpos,ypos,width-2,height-2,decodedimage);
+
+            free(decodedimage);
+    }
+
+    return 0;
+}
+
+static int saveBA81(FILE_HANDLE* handle, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame)
+{
+    uint16_t x, y;
+    uint8_t r, g, b;
+
+    uint32_t fpos = handle->fpos;
+	uint32_t row_size_padded = ((width-2)*3 + 3) & (~3); //row size aligned to 4 bytes
+	uint32_t fpos_end = fpos + row_size_padded* (height-2);
+
+
+    // skip first line
+    frame += width;
+
+    // don't render top and bottom rows, and left and rightmost columns because of color
+	// interpolation
+
+	for (y=1; y<height-1; y++)
+	{
+			frame++;
+			uint8_t rowbuf[row_size_padded];
+
+			//Bitmaps are saved "bottom-up". Seek to the right row.
+			if(filesystem_file_seek(handle,fpos_end-row_size_padded*y)!=F_OK) {
+				return -1;
+			}
+
+			for (x=1; x<width-1; x++, frame++)
+			{
+				interpolateBayer(width, x, y, frame, &r, &g, &b);
+				//bitmaps are saved in 24bit b,g,r format
+				rowbuf[(x-1)*3] = b;
+				rowbuf[(x-1)*3+1] = g;
+				rowbuf[(x-1)*3+2] = r;
+			}
+
+			if(filesystem_file_write(handle,rowbuf,row_size_padded)!=F_OK) {
+				return -1;
+			}
+
+			frame++;
+	 }
+	return 0;
+}
+
+
+int pixy_cc_set_region(uint8_t signum, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height) {
+	int32_t response;
+
+	int return_value = pixy_command("cc_setSigRegion",  // String id for remote procedure
+								  INT32(0),     // type = normal color code
+								  INT8(signum),
+								  INT16(xoffset),        // xoffset
+								  INT16(yoffset),         // yoffset
+								  INT16(width),       // width
+								  INT16(height),       // height
+								  END_OUT_ARGS,              // separator
+								  &response,      // pointer to mem address for return value
+								  END_IN_ARGS);
+	return return_value;
+}
+
+

common/app/pixy_helper.h

@@ -0,0 +1,59 @@
+#ifndef PIXY_HELPER_H
+#define PIXY_HELPER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "filesystem.h"
+
+/**
+ * Receives a fullsized frame from pixy and display's it on the display with the topleft corner at (x,y)
+ * @param x The x-Coordinate of the top left corner
+ * @param y The y-Coordinate of the top left corner
+ * @return 0 on success, otherwise the errorcode from pixy
+ */
+int pixy_render_full_frame(uint16_t x, uint16_t y);
+
+/**
+ * Receives a cropped frame from pixy and display's it on the display with the topleft corner at (x,y)
+ * @param x The x-Coordinate of the top left corner to draw the image
+ * @param y The y-Coordinate of the top left corner to draw the image
+ * @param xoffset The x-Coordinate on the pixy image from where on you want the frame data
+ * @param yoffset The y-Coordinate on the pixy image from where on you want the frame data
+ * @param width The width of the image recorded from pixy
+ * @param height The height of the image recorded from pixy
+ * @return 0 on success, otherwise the errorcode from pixy
+ */
+int pixy_render_cropped_frame(uint16_t x, uint16_t y, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height);
+
+/**
+ * Receives a fullsized frame from pixy and saves it to the given file in the 24bit (b,g,a) format.
+ * Use this method to write the bitmap-data part of a windows bitmap (.bmp).
+ * This method will neither open nor close the passed file.
+ * @param handle The file to write the data to. The file must be open and it should be seeked to the right position.
+ * @return 0 on success, otherwise the errorcode from pixy
+ */
+int pixy_save_full_frame(FILE_HANDLE* handle);
+
+/**
+ * Receives a cropped frame from pixy and saves it to the given file in the 24bit (b,g,a) format.
+ * @param handle The file to write the data to. The file must be open and it should be seeked to the right position.
+ * @param xoffset The x-Coordinate on the pixy image from where on you want the frame data
+ * @param yoffset The y-Coordinate on the pixy image from where on you want the frame data
+ * @param width The width of the image recorded from pixy
+ * @param height The height of the image recorded from pixy
+ * @return 0 on success, otherwise the errorcode from pixy
+ */
+int pixy_save_cropped_frame(FILE_HANDLE* handle, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height);
+
+/**
+ * Sets the color signature  to the color in the selected region of the frame
+ * @param signum the color signature number (1..7)
+ * @param xoffset The x-Coordinate of the topleft point of the region
+ * @param yoffset The y-Coordinate of the topleft point of the region
+ * @param width The width of the region
+ * @param height The height of the region
+ * @return 0 on success, otherwise the errorcode from pixy
+ */
+int pixy_cc_set_region(uint8_t signum, uint16_t xoffset, uint16_t yoffset, uint16_t width, uint16_t height);
+
+#endif /* PIXY_HELPER_H */

common/app/screen_filetest.c

@@ -0,0 +1,139 @@
+#include "screen_filetest.h"
+#include "button.h"
+#include "tft.h"
+#include "filesystem.h"
+#include <stdlib.h>
+
+
+static BUTTON_STRUCT b_back;
+
+static void b_back_cb(void* button) {
+        gui_screen_back();
+}
+
+
+static void image_test();
+
+static void enter(void* screen) {
+	tft_clear(HEX(0xBABECD));
+	
+	//Back button
+	b_back.base.x1=10; //Start X of Button
+        b_back.base.y1=200; //Start Y of Button
+        b_back.base.x2=AUTO; //b_back.base.x1+160; //Auto Calculate X2 with String Width
+        b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+        b_back.txtcolor=WHITE; //Set foreground color
+        b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+        b_back.font=0; //Select Font
+        b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+        b_back.callback=b_back_cb; //Call b_back_cb as Callback
+        gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+
+/*
+	//tft test
+	tft_draw_pixel(0,0,BLACK);
+	tft_draw_pixel(319,239,BLACK);
+	tft_draw_pixel(10,210,BLUE);
+        tft_draw_pixel(12,210,BLUE);
+        tft_draw_rectangle(40,100,60,235,BLUE);
+        tft_fill_rectangle(100,215,200,225,GREEN);
+        tft_draw_line(10,50,310,225,RGB(0xFF,0,0xFF));
+        tft_draw_circle(10,10,100, RED);
+        tft_print_line(30, 130, RED, BLUE, 0, "Hallo");
+*/
+
+	tft_draw_line(10,30,310,30,BLACK);
+	tft_print_line(10,18,BLUE,TRANSPARENT,0,"Name         D H RO Date   Time     Size");
+	
+	int y = 33;	
+	
+	DIRECTORY_STRUCT* dir = filesystem_dir_open(".");
+	if(dir==NULL) return;
+
+	for(int i=0; i<dir->num_files; i++) {
+		FILE_STRUCT* file = &(dir->files[i]);
+		tft_print_formatted(10,y,
+			(file->fattrib&F_DIR)?GREEN:RED,
+			TRANSPARENT,0,"%-13s%c %c %s %02u%02u%02u %02u:%02u:%02u %u",
+			file->fname,
+			(file->fattrib&F_DIR)?'D':' ',
+			(file->fattrib&F_HID)?'H':' ',
+			(file->fattrib&F_RDO)?"R ":"RW",
+			file->fdate.day,
+			file->fdate.month,
+			(file->fdate.year+1980)%100,
+			file->ftime.hour,
+			file->ftime.min,
+			file->ftime.sec*2,
+			file->fsize);
+		y+=14;
+	}
+
+	filesystem_dir_close(dir);
+
+	y+=14;
+
+	FILE_HANDLE* file = filesystem_file_open("test.txt");
+	if(file==NULL) {
+		tft_print_line(10,y,BLUE,TRANSPARENT,0,"Could not open test.txt");
+	} else {
+		char buf [30];
+		int size = (file->fsize>30)?29:file->fsize-1;
+		FILE_STATUS st = filesystem_file_read(file,buf,size);
+	
+		if(st==F_OK) {
+			buf[file->fpos]='\0';
+			tft_print_formatted(10,y,BLUE,TRANSPARENT,0,"test.txt contains \"%s\"",buf);
+			long num = strtol(&(buf[file->fpos-4]),NULL,0);
+			num++;
+			
+			y+=14;
+			
+			if(filesystem_file_seek(file,file->fpos-4)!=F_OK) {
+				tft_print_formatted(10,y,BLUE,TRANSPARENT,0,"Could not seek to %d",file->fpos-4);
+			} else {
+				sprintf(buf,"%04d",num);
+				if(filesystem_file_write(file,buf,4) != F_OK) {
+					tft_print_formatted(10,y,BLUE,TRANSPARENT,0,"Could not write new number %d",num);
+				} else {
+					tft_print_formatted(10,y,BLUE,TRANSPARENT,0,"New number written %d",num);
+				}
+			}
+		} else {
+			tft_print_line(10,y,BLUE,TRANSPARENT,0,"Could not read from test.txt");
+		}
+
+	}
+	filesystem_file_close(file);
+
+	image_test();	
+}
+
+static void leave(void* screen) {
+	gui_button_remove(&b_back);
+}
+
+static void update(void* screen) {
+}
+
+
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_filetest() {
+	return &screen;
+}
+
+static void image_test() {
+
+
+	if(!tft_draw_bitmap_file_unscaled(250,170,"cpu.bmp")) {
+		tft_print_line(10,180,BLUE,TRANSPARENT,0,"Could not open cpu.bmp");
+	}
+	tft_draw_rectangle(250-1,170-1,250-1+64,170-1+64,BLACK);
+}

common/app/screen_filetest.h

@@ -0,0 +1,23 @@
+#include "screen.h"
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup filetest Filetest (Screen)
+ * The File-Test Screen tests the filesystem module. It read/writes from/to files and shows a bitmap
+ */
+/*@{*/
+
+
+/**
+ * Returns a pointer to the filetest screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_filetest();
+
+/*@}*/
+/*@}*/

common/app/screen_guitest.c

@@ -0,0 +1,135 @@
+#include "screen_guitest.h"
+#include "button.h"
+#include "tft.h"
+#include "checkbox.h"
+#include "numupdown.h"
+
+static BUTTON_STRUCT b_back;
+static TOUCH_AREA_STRUCT a_area;
+static CHECKBOX_STRUCT c_cbox;
+static NUMUPDOWN_STRUCT n_updown;
+
+static void checkboxCB(void *checkbox, bool checked) {
+        printf("Checkbox %s\n",(checked?"checked":"unchecked"));
+}
+
+static void b_back_cb(void* button) {
+        gui_screen_back();
+}
+
+static void n_updown_cb(void* numupdown, int16_t value) {
+        printf("New NumUpDown Value %d\n",value);
+}
+
+static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
+        
+        switch(triggeredAction) {
+                case PEN_DOWN:
+                        printf("action PEN_DOWN\n");
+                break;
+                case PEN_UP:
+                        printf("action PEN_UP\n");
+                break;
+                case PEN_MOVE:
+                        printf("action PEN_MOVE\n");
+                break;
+                case PEN_ENTER:
+                        printf("action PEN_ENTER\n");
+                break;
+                case PEN_LEAVE:
+                        printf("action PEN_LEAVE\n");
+                break;
+                default:        
+                printf("action %s\n",triggeredAction);
+                break;
+        }
+}
+
+static void enter(void* screen) {
+	tft_clear(HEX(0xA6FD9A));
+	
+	//Back button
+	b_back.base.x1=10; //Start X of Button
+        b_back.base.y1=10; //Start Y of Button
+        b_back.base.x2=AUTO; //b_back.base.x1+160; //Auto Calculate X2 with String Width
+        b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+        b_back.txtcolor=WHITE; //Set foreground color
+        b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+        b_back.font=0; //Select Font
+        b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+        b_back.callback=b_back_cb; //Call b_back_cb as Callback
+        gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+
+	//tft test
+	tft_draw_pixel(0,0,BLACK);
+	tft_draw_pixel(319,239,BLACK);
+	tft_draw_pixel(10,210,BLUE);
+        tft_draw_pixel(12,210,BLUE);
+        tft_draw_rectangle(40,100,60,235,BLUE);
+        tft_fill_rectangle(100,215,200,225,GREEN);
+        tft_draw_line(10,50,310,225,RGB(0xFF,0,0xFF));
+        tft_draw_circle(10,10,100, RED);
+        tft_print_line(30, 130, RED, BLUE, 0, "Hallo");
+	
+
+
+	//Area test
+	a_area.hookedActions = PEN_DOWN | PEN_UP | PEN_MOVE | PEN_ENTER | PEN_LEAVE;
+        a_area.x1 = 130;
+        a_area.y1 = 30;
+        a_area.x2 = 200;
+        a_area.y2 = 60;
+        a_area.callback = touchCB;
+        touch_register_area(&a_area);
+
+
+
+	 //Checkbox test
+        c_cbox.base.x1=220;
+        c_cbox.base.y1=45;
+        c_cbox.base.x2=c_cbox.base.x1+16;
+        c_cbox.base.y2=c_cbox.base.y1+16;
+        c_cbox.fgcolor = GREEN;
+        c_cbox.checked = true;
+        c_cbox.callback = checkboxCB;
+        gui_checkbox_add(&c_cbox);
+	
+
+	//Num up down test
+	n_updown.x=200;
+	n_updown.y=120;
+	n_updown.fgcolor=RED;
+	n_updown.value = -3;
+	n_updown.max=11;
+	n_updown.min =-5;
+	n_updown.callback=n_updown_cb;
+	gui_numupdown_add(&n_updown);
+
+}
+
+static void leave(void* screen) {
+	gui_button_remove(&b_back);
+	gui_checkbox_remove(&c_cbox);
+	gui_numupdown_remove(&n_updown);
+	touch_unregister_area(&a_area);
+}
+
+static void update(void* screen) {
+	//gui_button_redraw(&b_back); //only needed if button is overdrawn by others
+	//.... for the other elements as well
+}
+
+
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_guitest() {
+	return &screen;
+}
+
+

common/app/screen_guitest.h

@@ -0,0 +1,24 @@
+#include "screen.h"
+
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup guitest Guitest (Screen)
+ * The Gui-Test Screen tests the gui and the tft module.
+ */
+/*@{*/
+
+
+/**
+ * Returns a pointer to the guitest screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_guitest();
+
+/*@}*/
+/*@}*/

common/app/screen_main.c

@@ -0,0 +1,173 @@
+#include "screen_main.h"
+#include "screen_guitest.h"
+#include "screen_pixytest.h"
+#include "screen_filetest.h"
+#include "screen_photomode.h"
+#include "screen_tracking.h"
+#include "button.h"
+#include "tft.h"
+#include "filesystem.h"
+
+BUTTON_STRUCT b_guitest;
+BUTTON_STRUCT b_pixytest;
+BUTTON_STRUCT b_filetest;
+
+BUTTON_STRUCT b_our_tracking;
+BUTTON_STRUCT b_ref_tracking;
+BUTTON_STRUCT b_photo_mode;
+
+
+static void b_our_tracking_cb(void* button) {
+	tracking_set_mode(OUR_TRACKING);
+	gui_screen_navigate(get_screen_tracking());
+}
+
+static void b_ref_tracking_cb(void* button) {
+	tracking_set_mode(REFERENCE_TRACKING);
+	gui_screen_navigate(get_screen_tracking());
+}
+
+static void b_photo_mode_cb(void* button) {
+	gui_screen_navigate(get_screen_photomode());
+}
+
+static void b_guitest_cb(void* button) {
+	gui_screen_navigate(get_screen_guitest());
+}
+
+static void b_filetest_cb(void* button) {
+	gui_screen_navigate(get_screen_filetest());
+}
+
+static void b_pixytest_cb(void* button) {
+	gui_screen_navigate(get_screen_pixytest());
+}
+
+
+static void enter(void* screen) {
+	tft_clear(WHITE);
+
+	//Heading
+	tft_print_line(10,10,BLUE,TRANSPARENT,1,"Discoverpixy");
+	tft_draw_line(0,40,319,40,BLACK);
+
+    #define X_TAB 			97
+	#define BUTTON_SPACING 	7
+
+	//First line of buttons
+	#define Y_FIRST 		60
+	tft_print_line(10,Y_FIRST,BLACK,TRANSPARENT,0,"Tracking:");
+
+	b_our_tracking.base.x1=X_TAB; //Start X of Button
+	b_our_tracking.base.y1=Y_FIRST-3; //Start Y of Button
+	b_our_tracking.base.x2=AUTO; //Auto Calculate X2 with String Width
+	b_our_tracking.base.y2=AUTO; //Auto Calculate Y2 with String Height
+	b_our_tracking.txtcolor=WHITE; //Set foreground color
+	b_our_tracking.bgcolor=HEX(0xE30535); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+	b_our_tracking.font=0; //Select Font
+	b_our_tracking.text="Our Tracking"; //Set Text (For formatted strings take sprintf)
+	b_our_tracking.callback=b_our_tracking_cb; //Call b_our_tracking when the button get's pressed
+	gui_button_add(&b_our_tracking); //Register Button (and run the callback from now on)
+
+
+	b_ref_tracking.base.x1=b_our_tracking.base.x2+BUTTON_SPACING;
+	b_ref_tracking.base.y1=Y_FIRST-3;
+	b_ref_tracking.base.x2=AUTO;
+	b_ref_tracking.base.y2=AUTO;
+	b_ref_tracking.txtcolor=WHITE;
+	b_ref_tracking.bgcolor=HEX(0xFF2151);
+	b_ref_tracking.font=0;
+	b_ref_tracking.text="Ref Tracking";
+	b_ref_tracking.callback=b_ref_tracking_cb;
+	gui_button_add(&b_ref_tracking);
+
+	//Second line of buttons
+	#define Y_SECOND Y_FIRST+25
+	tft_print_line(10,Y_SECOND,BLACK,TRANSPARENT,0,"Photo mode:");
+
+	b_photo_mode.base.x1=X_TAB;
+	b_photo_mode.base.y1=Y_SECOND-3;
+	b_photo_mode.base.x2=AUTO;
+	b_photo_mode.base.y2=AUTO;
+	b_photo_mode.txtcolor=WHITE;
+	b_photo_mode.bgcolor=HEX(0x21B1FF);
+	b_photo_mode.font=0;
+	b_photo_mode.text="Photo Mode";
+	b_photo_mode.callback=b_photo_mode_cb;
+	gui_button_add(&b_photo_mode);
+
+
+	//Third line of buttons
+	#define Y_THIRD Y_SECOND+25
+	tft_print_line(10,Y_THIRD,BLACK,TRANSPARENT,0,"Tests:");
+
+	b_guitest.base.x1=X_TAB;
+	b_guitest.base.y1=Y_THIRD-3;
+	b_guitest.base.x2=AUTO;
+	b_guitest.base.y2=AUTO;
+	b_guitest.txtcolor=BLACK;
+	b_guitest.bgcolor=HEX(0x00FA21);
+	b_guitest.font=0;
+	b_guitest.text="Gui & Tft";
+	b_guitest.callback=b_guitest_cb;
+	gui_button_add(&b_guitest);
+
+
+	b_pixytest.base.x1=b_guitest.base.x2+BUTTON_SPACING;
+	b_pixytest.base.y1=Y_THIRD-3;
+	b_pixytest.base.x2=AUTO;
+	b_pixytest.base.y2=AUTO;
+	b_pixytest.txtcolor=BLACK;
+	b_pixytest.bgcolor=HEX(0x00FA96);
+	b_pixytest.font=0;
+	b_pixytest.text="Pixy";
+	b_pixytest.callback=b_pixytest_cb;
+	gui_button_add(&b_pixytest);
+
+
+	b_filetest.base.x1=b_pixytest.base.x2+BUTTON_SPACING;
+	b_filetest.base.y1=Y_THIRD-3;
+	b_filetest.base.x2=AUTO;
+	b_filetest.base.y2=AUTO;
+	b_filetest.txtcolor=BLACK;
+	b_filetest.bgcolor=HEX(0x00FAC4);
+	b_filetest.font=0;
+	b_filetest.text="File";
+	b_filetest.callback=b_filetest_cb;
+	gui_button_add(&b_filetest);
+
+
+	//Bottom line
+	tft_draw_line(0,145,319,145,BLACK);
+	tft_print_line(10,150,BLUE,TRANSPARENT,0,"Powered by");
+	tft_draw_bitmap_file_unscaled(10,165,"pixy_small.bmp");
+	tft_draw_bitmap_file_unscaled(165,165,"stm_small.bmp");
+
+}
+
+static void leave(void* screen) {
+	gui_button_remove(&b_our_tracking);
+	gui_button_remove(&b_ref_tracking);
+	gui_button_remove(&b_photo_mode);
+	gui_button_remove(&b_guitest);
+	gui_button_remove(&b_pixytest);
+	gui_button_remove(&b_filetest);
+}
+
+static void update(void* screen) {
+	//gui_button_redraw(&b_guitest); //only needed if button is overdrawn by others
+}
+
+
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_main() {
+	return &screen;
+}
+
+

common/app/screen_main.h

@@ -0,0 +1,35 @@
+#include "screen.h"
+
+/**
+ * @addtogroup app
+ */
+/*@{*/
+
+/**
+ * @defgroup screens Screens
+ * The Screens of the application. \sa \ref screen
+ */
+/*@}*/
+
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup main Main (Screen)
+ * The Main Screen is the start-screen for the application
+ */
+/*@{*/
+
+
+/**
+ * Returns a pointer to the main screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_main();
+
+/*@}*/
+/*@}*/

common/app/screen_photomode.c

@@ -0,0 +1,166 @@
+#include "screen_photomode.h"
+#include "screen_photomode_save.h"
+#include "button.h"
+#include "tft.h"
+#include "touch.h"
+#include "pixy.h"
+#include "system.h"
+#include "pixy_helper.h"
+
+static bool pixy_connected = false; //Whether or not the pixy cam is currently connected
+
+static BUTTON_STRUCT b_back; //Button to navigate back
+static BUTTON_STRUCT b_save; //Button to save the current image
+static TOUCH_AREA_STRUCT a_area; //Touch Area, where the frame is drawn. Used to drag the image around
+static bool subMenu=false; //Whether or not we left the current screen for a submenu
+
+//Callback for when the user presses the "back" button
+static void b_back_cb(void* button) {
+	subMenu = false; //we're not entering a submenu
+	gui_screen_back(); //navigate back to the previous screen
+}
+
+//Callback for when the user presses the "save" button
+static void b_save_cb(void* button) {
+	subMenu = true; //we're entering a submenu
+	gui_screen_navigate(get_screen_photomodesave()); //navigate to the save screen
+}
+
+static POINT_STRUCT pixy_pos; //The current position of pixy's servos
+static POINT_STRUCT old_pos; //The last touch position on the screen
+
+//Callback for when the user drags the image around
+static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
+	POINT_STRUCT p = touch_get_last_point(); //get the last touched point
+	switch(triggeredAction) {
+		case PEN_ENTER:
+        case PEN_DOWN:
+			old_pos = p; //If the user "newly" enters the touch area, we set the "last" position to the current
+            break;
+        case PEN_MOVE: //the user is moving around, he entered the screen a while ago (old_pos is set)
+		{
+			int16_t deltaX = p.x - old_pos.x; //Calculate x difference between last and current touch
+			int16_t deltaY = p.y - old_pos.y; //Calculate y difference between last and current touch
+			old_pos=p; //store the current touch point for the next time
+			//printf("%d %d\n",deltaX,deltaY);
+			if(pixy_connected) {
+				//Calculate new servo coordinates. 2 is just a proportional factor
+				int16_t new_x = pixy_pos.x+deltaX*2;
+				int16_t new_y = pixy_pos.y-deltaY*2;
+
+				//check limits
+				if(new_x<0) new_x=0;
+				if(new_x>1000) new_x=1000;
+				if(new_y<0) new_y=0;
+				if(new_y>1000) new_y=1000;
+
+				//set pixy_pos so that the main routine can send it to the servos
+				pixy_pos.x = new_x;
+				pixy_pos.y= new_y;
+			}
+		}
+        break;
+		case PEN_UP:
+		case PEN_LEAVE:
+			//printf("Leave/up\n");
+			break;
+		default: break;
+	}
+	
+}
+
+//Callback for when the screen is entered/loaded
+static void enter(void* screen) {
+	tft_clear(WHITE);
+	
+	tft_print_line(5,5,BLACK,TRANSPARENT,0,"Drag the image around and ");
+
+	//Back button
+	b_back.base.x1=5; //Start X of Button
+	b_back.base.y1=19; //Start Y of Button
+	b_back.base.x2=AUTO; //Auto Calculate X2 with String Width
+	b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+	b_back.txtcolor=WHITE; //Set foreground color
+	b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+	b_back.font=0; //Select Font
+	b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+	b_back.callback=b_back_cb; //Call b_back_cb as Callback
+	gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+	//Save button
+	b_save.base.x1=190;
+	b_save.base.y1=3;
+	b_save.base.x2=AUTO;
+	b_save.base.y2=AUTO;
+	b_save.txtcolor=WHITE;
+	b_save.bgcolor=HEX(0x1010AE);
+	b_save.font=0;
+	b_save.text="Save it!";
+	b_save.callback=b_save_cb;
+	gui_button_add(&b_save);
+
+	//Frame Coordinates: topleft = (1,40); bottomright = (318,238)
+	//Leave a 10px border for the area
+
+	//Area to drag the image around
+	a_area.hookedActions =  PEN_DOWN | PEN_MOVE |  PEN_ENTER | PEN_UP | PEN_LEAVE;
+	a_area.x1 = 11;
+	a_area.y1 = 50;
+	a_area.x2 = 308;
+	a_area.y2 = 228;
+	a_area.callback = touchCB;
+	touch_register_area(&a_area);
+
+	//Pixy stuff
+    pixy_connected = (pixy_init()==0); //try to connect to pixy
+	if(pixy_connected && !subMenu) { //pixy is connected, but we are not coming from a submenu
+		pixy_pos.x=pixy_pos.y=500; //reset servo positions to center
+	}
+}
+
+//Callback for when the screen is left/unloaded
+static void leave(void* screen) {
+	//remove buttons and touch area.
+	gui_button_remove(&b_back);
+	gui_button_remove(&b_save);
+	touch_unregister_area(&a_area);
+}
+
+//Callback for when the screen should be updated
+//This is the main loop of the screen. This method will be called repeatedly
+static void update(void* screen) {
+  //Note: The only way to detect that pixy has been disconnected is if a command fails. There's no pixy_is_connected method yet :'(
+
+	if(!pixy_connected) { //Pixy not connected
+		pixy_close(); //Ensure that all pixy resources are freed (failsafe)
+		if(pixy_init()==0) { //try to connect to pixy
+			pixy_connected=true;
+			if(!subMenu) { //we're not coming from a submenu
+				pixy_pos.x=pixy_pos.y=500; //reset servo positions to center
+			}
+			printf("pixy (re)initialized\n");
+		}
+	}
+
+	if(pixy_connected) { //If we are connected (now)
+		pixy_service(); //Handle pending pixy events (e.g. color info retrival)
+
+		pixy_render_full_frame(1,40); //render the pixy video at point (1,40)
+	
+		//set the servo positions to the coordinates form the touch interrupt
+		pixy_rcs_set_position(0,pixy_pos.x);
+		pixy_rcs_set_position(1,pixy_pos.y);
+	}
+}
+
+//Declare screen callbacks
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_photomode() {
+	return &screen;
+}

common/app/screen_photomode.h

@@ -0,0 +1,22 @@
+#include "screen.h"
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup photomode Photo Mode (Screen)
+ * The Photo Mode Screen allows taking snapshots of the current pixy cam feed
+ */
+/*@{*/
+
+/**
+ * Returns a pointer to the photomode screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_photomode();
+
+/*@}*/
+/*@}*/

common/app/screen_photomode_save.c

@@ -0,0 +1,301 @@
+#include "screen_photomode_save.h"
+#include "filesystem.h"
+#include "button.h"
+#include "tft.h"
+#include "touch.h"
+#include "pixy.h"
+#include "pixy_helper.h"
+#include <stdlib.h>
+#include <string.h>
+
+
+static BUTTON_STRUCT b_back; //Button to navigate back
+static TOUCH_AREA_STRUCT a_area; //Touch area to select the save-file
+
+//Callback for when the user presses the "back" button
+static void b_back_cb(void* button) {
+        gui_screen_back();
+}
+
+static int num_files_ok; //number of files into which we can write the image (size, flags ok)
+static enum {init, error, showlist, picking, saving, done} state; //Current state of the screen state machine
+static int fontheight; //The space between one line of text to the next
+static int liststart; //The y-Coordinate of the Start of the File-List
+static const char* picked_file; //The filename picked by the user, to save the image to
+
+//Linked list structure to save all files which are suitable for saving.
+typedef struct FILE_LIST_ENTRY_S{
+	char* filename; //Name of the file
+	struct FILE_LIST_ENTRY_S* next; //Pointer to the next entry in the list or NULL
+} FILE_LIST_ENTRY;
+
+static FILE_LIST_ENTRY* files_ok; //Pointer to the head of the list
+
+//Callback for when the user selects a file to save the image into
+static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
+
+	int y = touch_get_last_point().y-liststart; //Calculate the y-Coordinate of the touch point relative to the start of the file-list
+	int elem = y/fontheight; //Calculate the file index
+	if(elem<0 | elem>= num_files_ok) return; //Check if the file index is valid  (0,1,..,num_files_ok-1)
+
+	//Search for the corresponding entry in the linked list
+	FILE_LIST_ENTRY* current_entry = files_ok; //Start walking through the list, starting by the head of the list
+	for(int i=0; i<elem; i++) { //Until we have reached the file (index)
+		current_entry= current_entry->next; //traverse to the next file
+	}
+
+	picked_file = current_entry->filename; //save the picked filename. It will be used by the statemachine in the main loop
+	touch_unregister_area(&a_area); //unregister the touch area, we no longer need it. No more interrupts will be fired.
+	state=saving; //Change the state of the  statemachine
+}
+
+//Text-Lines to show if we have no matching files (num_files_ok==0)
+static const char* nomatch_text [] = {
+	"Due to limitations of the filesystem",
+	"implementation you can only write to",
+	"existing files.",
+	"",
+	"The files need to have a .bmp",
+	"extension and must be at least",
+	"189410 bytes (185kb) large.",
+	"Unfortunately there were no such",
+	"files found in the root directory.",
+	"",
+	"Please create some files and come",
+	"back again.",
+	NULL
+};
+
+
+//Bitmap header for a 318x198x24bit windows bitmap. data starts at 0x7A (= after this header)
+//This header has been taken from a white bitmap saved with gimp.
+//Wikipedia has a pretty good description on the header: http://de.wikipedia.org/wiki/Windows_Bitmap
+static unsigned char bmpheader_data[0x7A] = {
+		  0x42, 0x4d, 0xe2, 0xe3, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7a, 0x00,
+		  0x00, 0x00, 0x6c, 0x00, 0x00, 0x00, 0x3e, 0x01, 0x00, 0x00, 0xc6, 0x00,
+		  0x00, 0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x68, 0xe3,
+		  0x02, 0x00, 0x13, 0x0b, 0x00, 0x00, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x47, 0x52, 0x73, 0x00, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,
+		  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		  0x00, 0x00
+};
+
+//Callback for when the screen is entered/loaded
+static void enter(void* screen) {
+	tft_clear(WHITE);
+
+
+	#define X_OFS 5
+
+	//Back button
+	b_back.base.x1=X_OFS; //Start X of Button
+	b_back.base.y1=210; //Start Y of Button
+	b_back.base.x2=AUTO; //Auto Calculate X2 with String Width
+	b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+	b_back.txtcolor=WHITE; //Set foreground color
+	b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+	b_back.font=0; //Select Font
+	b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+	b_back.callback=b_back_cb; //Call b_back_cb as Callback
+	gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+	state =init; //Start with the init state
+	fontheight = tft_font_height(0)+2; //Save the height of the used font, for fast access
+	files_ok = NULL; //initialize the linked list with 0 elements
+	num_files_ok = 0; //we have zero! elements
+}
+
+//Callback for when the screen should be updated
+//This is the main loop of the screen. This method will be called repeatedly
+static void update(void* screen) {
+	switch(state) {
+		case init: //Init State: The user just entered the screen
+		{
+			DIRECTORY_STRUCT* dir = filesystem_dir_open("."); //open root directory
+			if(dir==NULL) { //error while opening root directory
+				tft_print_line(X_OFS,5,BLACK,TRANSPARENT,0,"Error accessing Filesystem");
+				state=error;
+				break;
+			}
+
+			bool nomatch= true; //whether or not we have zero files which are suitable for saving
+
+			for(int i=0; i<dir->num_files; i++) { //walk through all files in the directory
+					FILE_STRUCT* file = &(dir->files[i]); //Pointer to the current file/subdirectory
+
+					//Ignore directories, archives, hidden files, system files and files we cannot write to
+					if(file->fattrib&(F_SYS|F_HID|F_ARC|F_DIR|F_RDO)) continue;
+
+					//ignore files which are not large enough
+					if(file->fsize<189410) continue; //size taken from an example bitmap (318x198x24)
+
+					nomatch=false; //at least one file matches
+					break;
+			}
+
+			if(nomatch) { //not one file is suitable for writing
+				int y=5; //y-Coordinate where to start writing the error text
+				int i=0;
+				while(nomatch_text[i]!=NULL) { //for every line in the big error array
+					//Write the line's text and go to the next line
+					tft_print_line(X_OFS,y+i*fontheight,BLACK,TRANSPARENT,0,nomatch_text[i]);
+					i++;
+				}
+				state = error;
+			} else { //we have a least one suitable file
+				state = showlist;
+			}
+
+			filesystem_dir_close(dir); //free directory struct
+		}
+		break;
+
+		case showlist: //Show List State: Where we load and present the suitable file's to the user in a list
+		{
+			DIRECTORY_STRUCT* dir2 = filesystem_dir_open("."); //Open the directory again
+			if(dir2==NULL) return; //Error on opening? This should never happen, since it's handled in the previous state
+
+			int y = 5; //y-Coordinate where to start drawing/writing text/list-elements
+
+			tft_print_line(X_OFS,y,BLACK,TRANSPARENT,0,"Pick a file to save the image to");
+			y+=fontheight+5;
+
+			tft_print_line(X_OFS,y,BLUE,TRANSPARENT,0,"Name             Modified          Size");
+			y+=fontheight;
+
+			liststart = y; //store the y coordinate of the start of the list away (used in toucharea callback)
+			num_files_ok = 0; //we start with 0 matching files
+
+			FILE_LIST_ENTRY* current_entry = NULL; //We start with an empty list
+			for(int i=0; i<dir2->num_files && num_files_ok<10; i++) { //go through all the files of the directory, abort if we have 10 matches
+					FILE_STRUCT* file = &(dir2->files[i]);
+
+					//Ignore directories, archives, hidden files, system files and files we cannot write to
+					if(file->fattrib&(F_SYS|F_HID|F_ARC|F_DIR|F_RDO)) continue;
+
+					//ignore files which are not large enough
+					if(file->fsize<189410) continue; //size taken from an example bitmap (318x198x24)
+
+					//Print out filename, modified date,time and file size
+					tft_print_formatted(X_OFS,y,BLACK,
+								TRANSPARENT,0,"%-16s %02u.%02u.%02u %02u:%02u:%02u %u",
+								file->fname,
+								file->fdate.day,
+								file->fdate.month,
+								(file->fdate.year+1980)%100,
+								file->ftime.hour,
+								file->ftime.min,
+								file->ftime.sec*2,
+								file->fsize);
+
+					if(current_entry==NULL) { //The list is empty
+						current_entry = malloc(sizeof(FILE_LIST_ENTRY)); //create new entry
+						files_ok = current_entry; //assign it to the list head
+					} else { //there's a least one entry in the list
+						current_entry->next = malloc(sizeof(FILE_LIST_ENTRY)); //append entry to previous entry
+						current_entry = current_entry->next; //newly created entry is the current now.
+					}
+					current_entry->next = NULL; //we're at the end of the list (for now)
+					current_entry->filename = malloc(strlen(file->fname)+1); //allocate space for the filename + zero-termination
+					strcpy(current_entry->filename,file->fname); //copy filename (so that we can close the directory after scanning)
+
+					//since we have found a suitable file we need to increment the position in the list
+					num_files_ok++;
+					y+=fontheight;
+			}
+
+			//Touch area for file-selection (in the list)
+			a_area.hookedActions = PEN_UP; //we're only interested in PEN_UP events
+			a_area.x1 = X_OFS; //Left border
+			a_area.y1 = liststart; //Start where the list started
+			a_area.x2 = 320-X_OFS; //Right border
+			a_area.y2 = liststart+fontheight*num_files_ok; //stop at the end of the list
+			a_area.callback = touchCB; //execute our callback when PEN_UP occurs
+			touch_register_area(&a_area); //register the touch area and receive events from now on
+
+			filesystem_dir_close(dir2); //we no longer need the directory struct, since we have our own linked list now
+
+			state=picking;
+		}
+		break;
+
+		case picking: //Picking State: Where we wait on the users file choice
+			pixy_service(); //Handle pending pixy events
+			//do nothing and wait on user to pick a file
+			break;
+
+		case saving: //Saving State: Where we save the image to the selected file
+		{
+			FILE_HANDLE* file = filesystem_file_open(picked_file); //try to open the selected file
+			if(file==NULL) { //opening the file failed
+				tft_print_formatted(X_OFS,190,BLUE,TRANSPARENT,0,"Could not open %s",picked_file);
+				state=error;
+				break;
+			}
+
+			filesystem_file_seek(file,0); //seek to the start of the file (optional?)
+			if(filesystem_file_write(file,bmpheader_data,0x7A)!=F_OK) { //Writing the header failed
+				tft_print_formatted(X_OFS,190,BLUE,TRANSPARENT,0,"Error while writing to %s",picked_file);
+				filesystem_file_close(file);
+				state=error;
+				break;
+			}
+
+			if(pixy_save_full_frame(file)!=0) { //Writing the imagedata failed
+				tft_print_formatted(X_OFS,190,BLUE,TRANSPARENT,0,"Error while writing to %s",picked_file);
+				filesystem_file_close(file);
+				state=error;
+				break;
+			}
+
+			//if we reach this point, we have written all data out successfully
+
+			filesystem_file_close(file); //close/finalize the file
+			tft_print_formatted(X_OFS,190,BLUE,TRANSPARENT,0,"Image saved to %s",picked_file);
+			state = done;
+		}
+		break;
+
+		case error: //Error State: Where we show an error message and leave the user no other choice than to click the backbutton
+		case done: //Done State: When saving the file was successful
+			pixy_service(); //Handle pending pixy events
+			//wait on user to click the back button
+			break;
+
+	}
+}
+
+//Callback for when the screen is left/unloaded
+static void leave(void* screen) {
+	gui_button_remove(&b_back); //Remove/Free the back button
+
+	if(state==picking){ //The user left the screen in the "picking"-phase
+		touch_unregister_area(&a_area); //remove the touch area (for the list)
+	}
+
+	if(state==picking|| state==saving || state==done) { //the user left the screen after we created the linked list
+		//Iterate through the linked list and free all resources
+		FILE_LIST_ENTRY* current_entry = files_ok; //start with the list head
+		while(current_entry!=NULL) { //while we're not at the end
+			FILE_LIST_ENTRY* temp = current_entry->next; //save the next pointer because we free the current element on the next line
+			free((void*)(current_entry->filename)); //free filename
+			free(current_entry); //free element itself
+			current_entry= temp; //advance
+		}
+	}
+}
+
+//Declare screen callbacks
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+SCREEN_STRUCT* get_screen_photomodesave() {
+	return &screen;
+}

common/app/screen_photomode_save.h

@@ -0,0 +1,23 @@
+#include "screen.h"
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup photomodesave Photo Mode Save (Screen)
+ * The Photo Mode Save Screen helps the user saving a file to the filesystem
+ */
+/*@{*/
+
+/**
+ * Returns a pointer to the photomode save screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_photomodesave();
+
+/*@}*/
+/*@}*/
+

common/app/screen_pixytest.c

@@ -0,0 +1,342 @@
+#include "screen_pixytest.h"
+#include "button.h"
+#include "numupdown.h"
+#include "tft.h"
+#include "touch.h"
+#include "pixy.h"
+#include "system.h"
+#include "pixy_helper.h"
+
+static volatile enum {detecting, idle,update_servos, update_ledcolor, update_ledcurrent} state; //Current state of the screen state machine
+
+static BUTTON_STRUCT b_back;
+
+static BUTTON_STRUCT b_servos_center;
+static BUTTON_STRUCT b_servos_topleft;
+static BUTTON_STRUCT b_servos_topright;
+static BUTTON_STRUCT b_servos_bottomleft;
+static BUTTON_STRUCT b_servos_bottomright;
+static uint16_t servo_x;
+static uint16_t servo_y;
+
+static BUTTON_STRUCT b_led_off;
+static BUTTON_STRUCT b_led_white;
+static BUTTON_STRUCT b_led_red;
+static BUTTON_STRUCT b_led_green;
+static BUTTON_STRUCT b_led_blue;
+static uint32_t led_color;
+
+static uint32_t led_maxcurrent;
+static NUMUPDOWN_STRUCT n_led_powerlimit;
+
+
+static void b_back_cb(void* button) {
+        gui_screen_back();
+}
+
+static void b_servos_center_cb(void* button) {
+		if(state==idle) {
+			servo_x=500;
+			servo_y=500;
+			state=update_servos;
+		}
+}
+
+static void b_servos_topleft_cb(void* button) {
+		if(state==idle) {
+			servo_x=0;
+			servo_y=0;
+			state=update_servos;
+		}
+}
+
+static void b_servos_topright_cb(void* button) {
+		if(state==idle) {
+			servo_x=1000;
+			servo_y=0;
+			state=update_servos;
+		}
+}
+
+static void b_servos_bottomleft_cb(void* button) {
+		if(state==idle) {
+			servo_x=0;
+			servo_y=1000;
+			state=update_servos;
+		}
+}
+
+static void b_servos_bottomright_cb(void* button) {
+		if(state==idle) {
+			servo_x=1000;
+			servo_y=1000;
+			state=update_servos;
+		}
+}
+
+static void b_led_off_cb(void* button) {
+		if(state==idle) {
+			led_color=0x000000;
+			state=update_ledcolor;
+		}
+}
+
+static void b_led_white_cb(void* button) {
+		if(state==idle) {
+			led_color=0xFFFFFF;
+			state=update_ledcolor;
+		}
+}
+
+static void b_led_red_cb(void* button) {
+		if(state==idle) {
+			led_color=0xFF0000;
+			state=update_ledcolor;
+		}
+}
+
+static void b_led_green_cb(void* button) {
+		if(state==idle) {
+			led_color=0x00FF00;
+			state=update_ledcolor;
+		}
+}
+
+static void b_led_blue_cb(void* button) {
+		if(state==idle) {
+			led_color=0x0000FF;
+			state=update_ledcolor;
+		}
+}
+
+static void n_led_powerlimit_cb(void* numupdown, int16_t value) {
+	if(state==idle) {
+		led_maxcurrent=value;
+		state=update_ledcurrent;
+	}
+}
+
+static void enter(void* screen) {
+	tft_clear(WHITE);
+	
+	//Back button
+	b_back.base.x1=10; //Start X of Button
+	b_back.base.y1=210; //Start Y of Button
+	b_back.base.x2=AUTO; //Auto Calculate X2 with String Width
+	b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+	b_back.txtcolor=WHITE; //Set foreground color
+	b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+	b_back.font=0; //Select Font
+	b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+	b_back.callback=b_back_cb; //Call b_back_cb as Callback
+	gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+
+	//Servo stuff
+	#define SERVO_BUTTON_Y 10
+	#define SERVO_BUTTON_SPACING 5
+	tft_print_line(5,SERVO_BUTTON_Y,BLACK,TRANSPARENT,0,"Servos:");
+
+	b_servos_center.base.x1=55;
+	b_servos_center.base.y1=SERVO_BUTTON_Y-3;
+	b_servos_center.base.x2=AUTO;
+	b_servos_center.base.y2=AUTO;
+	b_servos_center.txtcolor=WHITE;
+	b_servos_center.bgcolor=HEX(0xAE1010);
+	b_servos_center.font=0;
+	b_servos_center.text="Center";
+	b_servos_center.callback=b_servos_center_cb;
+	gui_button_add(&b_servos_center);
+
+	b_servos_topleft.base.x1=b_servos_center.base.x2+SERVO_BUTTON_SPACING;
+	b_servos_topleft.base.y1=SERVO_BUTTON_Y-3;
+	b_servos_topleft.base.x2=AUTO;
+	b_servos_topleft.base.y2=AUTO;
+	b_servos_topleft.txtcolor=WHITE;
+	b_servos_topleft.bgcolor=HEX(0xAE1010);
+	b_servos_topleft.font=0;
+	b_servos_topleft.text="ToLe";
+	b_servos_topleft.callback=b_servos_topleft_cb;
+	gui_button_add(&b_servos_topleft);
+
+	b_servos_topright.base.x1=b_servos_topleft.base.x2+SERVO_BUTTON_SPACING;
+	b_servos_topright.base.y1=SERVO_BUTTON_Y-3;
+	b_servos_topright.base.x2=AUTO;
+	b_servos_topright.base.y2=AUTO;
+	b_servos_topright.txtcolor=WHITE;
+	b_servos_topright.bgcolor=HEX(0xAE1010);
+	b_servos_topright.font=0;
+	b_servos_topright.text="ToRi";
+	b_servos_topright.callback=b_servos_topright_cb;
+	gui_button_add(&b_servos_topright);
+
+	b_servos_bottomleft.base.x1=b_servos_topright.base.x2+SERVO_BUTTON_SPACING;
+	b_servos_bottomleft.base.y1=SERVO_BUTTON_Y-3;
+	b_servos_bottomleft.base.x2=AUTO;
+	b_servos_bottomleft.base.y2=AUTO;
+	b_servos_bottomleft.txtcolor=WHITE;
+	b_servos_bottomleft.bgcolor=HEX(0xAE1010);
+	b_servos_bottomleft.font=0;
+	b_servos_bottomleft.text="BoLe";
+	b_servos_bottomleft.callback=b_servos_bottomleft_cb;
+	gui_button_add(&b_servos_bottomleft);
+
+	b_servos_bottomright.base.x1=b_servos_bottomleft.base.x2+SERVO_BUTTON_SPACING;
+	b_servos_bottomright.base.y1=SERVO_BUTTON_Y-3;
+	b_servos_bottomright.base.x2=AUTO;
+	b_servos_bottomright.base.y2=AUTO;
+	b_servos_bottomright.txtcolor=WHITE;
+	b_servos_bottomright.bgcolor=HEX(0xAE1010);
+	b_servos_bottomright.font=0;
+	b_servos_bottomright.text="BoRi";
+	b_servos_bottomright.callback=b_servos_bottomright_cb;
+	gui_button_add(&b_servos_bottomright);
+
+	//Led Color stuff
+	#define LED_COLOR_BUTTON_Y 35
+	#define LED_COLOR_BUTTON_SPACING 5
+	tft_print_line(5,LED_COLOR_BUTTON_Y,BLACK,TRANSPARENT,0,"Led Color:");
+
+	b_led_off.base.x1=85;
+	b_led_off.base.y1=LED_COLOR_BUTTON_Y-3;
+	b_led_off.base.x2=AUTO;
+	b_led_off.base.y2=AUTO;
+	b_led_off.txtcolor=WHITE;
+	b_led_off.bgcolor=BLACK;
+	b_led_off.font=0;
+	b_led_off.text="Off";
+	b_led_off.callback=b_led_off_cb;
+	gui_button_add(&b_led_off);
+
+	b_led_white.base.x1=b_led_off.base.x2+LED_COLOR_BUTTON_SPACING;
+	b_led_white.base.y1=LED_COLOR_BUTTON_Y-3;
+	b_led_white.base.x2=AUTO;
+	b_led_white.base.y2=AUTO;
+	b_led_white.txtcolor=BLACK;
+	b_led_white.bgcolor=HEX(0xEEEEEE);
+	b_led_white.font=0;
+	b_led_white.text="White";
+	b_led_white.callback=b_led_white_cb;
+	gui_button_add(&b_led_white);
+
+	b_led_red.base.x1=b_led_white.base.x2+LED_COLOR_BUTTON_SPACING;
+	b_led_red.base.y1=LED_COLOR_BUTTON_Y-3;
+	b_led_red.base.x2=AUTO;
+	b_led_red.base.y2=AUTO;
+	b_led_red.txtcolor=WHITE;
+	b_led_red.bgcolor=HEX(0xEE0000);
+	b_led_red.font=0;
+	b_led_red.text="Red";
+	b_led_red.callback=b_led_red_cb;
+	gui_button_add(&b_led_red);
+
+	b_led_green.base.x1=b_led_red.base.x2+LED_COLOR_BUTTON_SPACING;
+	b_led_green.base.y1=LED_COLOR_BUTTON_Y-3;
+	b_led_green.base.x2=AUTO;
+	b_led_green.base.y2=AUTO;
+	b_led_green.txtcolor=WHITE;
+	b_led_green.bgcolor=HEX(0x00EE00);
+	b_led_green.font=0;
+	b_led_green.text="Green";
+	b_led_green.callback=b_led_green_cb;
+	gui_button_add(&b_led_green);
+
+	b_led_blue.base.x1=b_led_green.base.x2+LED_COLOR_BUTTON_SPACING;
+	b_led_blue.base.y1=LED_COLOR_BUTTON_Y-3;
+	b_led_blue.base.x2=AUTO;
+	b_led_blue.base.y2=AUTO;
+	b_led_blue.txtcolor=WHITE;
+	b_led_blue.bgcolor=HEX(0x0000EE);
+	b_led_blue.font=0;
+	b_led_blue.text="Blue";
+	b_led_blue.callback=b_led_blue_cb;
+	gui_button_add(&b_led_blue);
+
+	//Led MaxPower stuff
+	#define LED_POWER_BUTTON_Y 70
+	tft_print_line(5,LED_POWER_BUTTON_Y,BLACK,TRANSPARENT,0,"Led Maximum Current:");
+
+	//Num up down test
+	n_led_powerlimit.x=160;
+	n_led_powerlimit.y=LED_POWER_BUTTON_Y-7;
+	n_led_powerlimit.fgcolor=WHITE;
+	n_led_powerlimit.value = 10;
+	n_led_powerlimit.max=40;
+	n_led_powerlimit.min =0;
+	n_led_powerlimit.callback=n_led_powerlimit_cb;
+	gui_numupdown_add(&n_led_powerlimit);
+
+
+
+	state=detecting;
+}
+
+static void leave(void* screen) {
+	gui_button_remove(&b_back);
+	gui_button_remove(&b_servos_center);
+	gui_button_remove(&b_servos_topleft);
+	gui_button_remove(&b_servos_topright);
+	gui_button_remove(&b_servos_bottomleft);
+	gui_button_remove(&b_servos_bottomright);
+	gui_button_remove(&b_led_off);
+	gui_button_remove(&b_led_white);
+	gui_button_remove(&b_led_red);
+	gui_button_remove(&b_led_green);
+	gui_button_remove(&b_led_blue);
+	gui_numupdown_remove(&n_led_powerlimit);
+
+}
+
+
+static void update(void* screen) {
+	switch(state) {
+		case detecting: //Detecting State: Where we try to connect to the pixy
+			if(pixy_init()==0) { //Pixy connection ok
+				int32_t response;
+				int return_value;
+				return_value = pixy_command("stop", END_OUT_ARGS,  &response, END_IN_ARGS);
+				pixy_led_set_max_current(10);
+
+				state = idle; //Go to next state
+			}
+			break;
+		case idle:
+			pixy_service();
+		break;
+		case update_servos:
+			pixy_rcs_set_position(0,servo_x);
+			pixy_rcs_set_position(1,servo_y);
+			state = idle;
+			break;
+
+		case update_ledcolor:
+		{
+			int32_t response;
+			int return_value;
+			return_value = pixy_command("led_set", INT32(led_color), END_OUT_ARGS,  &response, END_IN_ARGS);
+			state = idle;
+		}
+		break;
+
+		case update_ledcurrent:
+			pixy_led_set_max_current(led_maxcurrent);
+			state = idle;
+			break;
+	}
+
+}
+
+
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_pixytest() {
+	return &screen;
+}
+
+
+

common/app/screen_pixytest.h

@@ -0,0 +1,22 @@
+#include "screen.h"
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup pixytest Pixytest (Screen)
+ * The Pixy-Test Screen tests the pixy module.
+ */
+/*@{*/
+
+/**
+ * Returns a pointer to the pixytest screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_pixytest();
+
+/*@}*/
+/*@}*/

common/app/screen_tracking.c

@@ -0,0 +1,380 @@
+#include "screen_tracking.h"
+#include "pixy_control.h"
+#include "button.h"
+#include "checkbox.h"
+#include "tft.h"
+#include "touch.h"
+#include "pixy.h"
+#include "system.h"
+#include "pixy_helper.h"
+
+static BUTTON_STRUCT b_back; //Button to navigate back
+static BUTTON_STRUCT b_select; //Button to start the color region selection
+static CHECKBOX_STRUCT c_frame_toggle; //Checkbox to toggle video data on/off
+static TOUCH_AREA_STRUCT a_area; //Touch area for the color region selection
+
+//Callback for when the user presses the "back" button
+static void b_back_cb(void* button) {
+        gui_screen_back(); //navigate back to the previous screen
+}
+
+static volatile bool frame_visible = false; //Whether or not the video data should be displayed
+static void c_frame_toggle_cb(void *checkbox, bool checked) {
+	frame_visible=checked; //Set the visibility of the frame to the checked state of the checkbox
+	//Frame will be drawn in the main loop below
+}
+
+static enum {detecting, init, tracking, preselecting, abortselecting, selecting, selected, error} state; //Current state of the screen state machine
+
+static POINT_STRUCT point1; //First point of the rectangle selected by the user (color region selection)
+static POINT_STRUCT point2; //End point of the rectangle selected by the user (color region selection)
+static bool point1_valid; //Whether or not we have a valid first point
+
+//Callback for when the user presses the "select color" button
+static void b_select_cb(void* button) {
+	if(state==selecting) { //we're currently selecting a color region
+		state = abortselecting; //Abort selecting!!
+	} else if (state==tracking) { //we're currently watching the tracking
+		state = preselecting; //start selecting
+	}
+}
+
+//Video Region properties
+//The camera records with 320*200px, but we need to keep a 1px border because of color interpolation (bayer format)
+#define FRAME_START_X 1 //x-Coordinate of the top-left point of the frame rectangle on display
+#define FRAME_START_Y 41 //y-Coordinate of the top-left point of the frame rectangle on display
+#define FRAME_WIDTH 318 //Width of the video frame
+#define FRAME_HEIGHT 198 //Height of the video frame
+#define FRAME_END_X FRAME_START_X +FRAME_WIDTH-1 //x-Coordinate of the bottom-right point of the frame rectangle
+#define FRAME_END_Y FRAME_START_Y +FRAME_HEIGHT-1 //y-Coordinate of the bottom-right point of the frame rectangle
+
+//Callback for when the user touches the frame area to select a color region.
+//Note: It doesn't matter in which direction the user draws the rectangle, we'll normalize the coordinates later
+static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
+	POINT_STRUCT p = touch_get_last_point();
+	switch(triggeredAction) {
+		case PEN_DOWN: //The user just put down the pen
+			point1.x = p.x-FRAME_START_X; //Calculate x-Coordinate relative to frame start
+			point1.y = p.y-FRAME_START_Y; //Calculate y-Coordinate relative to frame start
+			point1_valid= true; //The point1 is now valid
+			break;
+		case PEN_UP: //The user took the pen away
+			if(point1_valid) { //only execute if point1 is valid
+				point2.x = p.x-FRAME_START_X;  //Calculate x-Coordinate relative to frame start
+				point2.y = p.y-FRAME_START_Y; //Calculate y-Coordinate relative to frame start
+				state = selected;
+			}
+			break;
+	}
+}
+
+//Prototype for tracking start/stop methods
+typedef void (*TRACKING_VOID_CALLBACK)(void* tracking_config);
+//Prototype for tracking update method
+typedef void (*TRACKING_BLOCK_CALLBACK)(void* tracking_config, struct Block* blocks, int num_blocks );
+
+//Structure to save callbacks and settings of a tracking implementation
+typedef struct {
+	TRACKING_VOID_CALLBACK start;
+	TRACKING_VOID_CALLBACK stop;
+	TRACKING_BLOCK_CALLBACK update;
+} TRACKING_CONFIG_STRUCT;
+
+//Methods for our tracking implementation ahead
+static int16_t servo_x = 0;
+static int16_t servo_y = 0;
+
+//Method/Callback to start our tracking
+void tracking_our_start(void* tracking_config) {
+	//Activate pixy's data send program
+	int32_t response;
+	int return_value;
+
+    servo_x = servo_y = 500;                // set a default value of 500
+    pixy_rcs_set_position(0, servo_x);      // set default 
+    pixy_rcs_set_position(1, servo_y);      // set default
+
+	return_value = pixy_command("runprog", INT8(0), END_OUT_ARGS,  &response, END_IN_ARGS);
+}
+
+//Method/Callback to stop our tracking
+void tracking_our_stop(void* tracking_config) {
+	//Stop pixy's data send programm
+	int32_t response;
+	int return_value;
+	return_value = pixy_command("stop", END_OUT_ARGS,  &response, END_IN_ARGS);
+}
+
+//Method/Callback to calculate one step of our tracking
+void tracking_our_update(void* tracking_config, struct Block* blocks, int num_blocks) {
+
+    if(num_blocks <= 0){                    // Check if there are blocks available
+        return;                             // When there are none, do nothing
+    }
+    
+    uint16_t x = blocks[0].x;               // Get x coordinate of the biggest object
+    uint16_t y = blocks[0].y;               // Get y coordinate of the biggest object
+
+    int16_t xset = 0;                       
+    int16_t yset = 0;
+
+    xset = (servo_x + pixy_PID_X((FRAME_WIDTH / 2), x));    // calculate the PID output for x
+    yset = (servo_y - pixy_PID_Y((FRAME_HEIGHT / 2), y));   // calculate the PID output for y
+    
+    xset = (xset < 0) ? 0 : xset;           // x lower boundary check
+    xset = (xset > 1000) ? 1000 : xset;     // x upper boundary check
+    
+    yset = (yset < 0) ? 0 : yset;           // y lower boundary check
+    yset = (yset > 1000) ? 1000 : yset;     // y upper boundary check
+
+    servo_x = xset;                         // update the global, static variable for x
+    servo_y = yset;                         // update the global, statuc variable for y
+
+    pixy_rcs_set_position(0, servo_x);      // set the new x position
+    pixy_rcs_set_position(1, servo_y);      // set the new y position
+}
+
+//Variable which stores all the callbacks and settings for our tracking implementation
+static TRACKING_CONFIG_STRUCT tracking_our = {
+		tracking_our_start,
+		tracking_our_stop,
+		tracking_our_update
+};
+
+//Methods for reference tracking implementation ahead
+
+//Method/Callback to start reference tracking
+void tracking_reference_start(void* tracking_config) {
+	//Run reference tracking
+	int32_t response;
+	int return_value;
+	return_value = pixy_command("runprog", INT8(2), END_OUT_ARGS,  &response, END_IN_ARGS);
+}
+
+//Method/Callback to stop reference tracking
+void tracking_reference_stop(void* tracking_config) {
+	//Stop reference tracking
+	int32_t response;
+	int return_value;
+	return_value = pixy_command("stop", END_OUT_ARGS,  &response, END_IN_ARGS);
+}
+
+//Method/Callback to calculate one step of the reference tracking
+void tracking_reference_update(void* tracking_config, struct Block* blocks, int num_blocks) {
+	//Nothing to do here. Pixy does it all.
+}
+
+//Variable which stores all the callbacks and settings for the reference tracking implementation
+static TRACKING_CONFIG_STRUCT tracking_reference = {
+		tracking_reference_start,
+		tracking_reference_stop,
+		tracking_reference_update
+};
+
+//Pointer to the currently active tracking implementation. See also tracking_set_mode
+static TRACKING_CONFIG_STRUCT* tracking_current;
+
+//Method to set the current tracking implementation. This function is exported and should be called before getting the screen
+void tracking_set_mode(enum Tracking_Implementation impl) {
+	//Depending on the enum value let tracking_current point to a different setting/callback structure
+	switch(impl) {
+		case OUR_TRACKING:
+			tracking_current = &tracking_our;
+			break;
+		case REFERENCE_TRACKING:
+			tracking_current = &tracking_reference;
+			break;
+		default:
+			tracking_current=NULL;
+			break;
+	}
+}
+
+//Callback for when the screen is entered/loaded
+static void enter(void* screen) {
+	tft_clear(WHITE);
+	
+	//"Back" button
+	b_back.base.x1=5; //Start X of Button
+	b_back.base.y1=5; //Start Y of Button
+	b_back.base.x2=AUTO; //Auto Calculate X2 with String Width
+	b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
+	b_back.txtcolor=WHITE; //Set foreground color
+	b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
+	b_back.font=0; //Select Font
+	b_back.text="Back"; //Set Text (For formatted strings take sprintf)
+	b_back.callback=b_back_cb; //Call b_back_cb as Callback
+	gui_button_add(&b_back); //Register Button (and run the callback from now on)
+
+
+	//"Select color" button
+	b_select.base.x1=150;
+	b_select.base.y1=5;
+	b_select.base.x2=AUTO;
+	b_select.base.y2=AUTO;
+	b_select.txtcolor=WHITE;
+	b_select.bgcolor=HEX(0xAE1010);
+	b_select.font=0;
+	b_select.text="Select Color";
+	b_select.callback=b_select_cb;
+	gui_button_add(&b_select);
+
+	//"Frame visible" checkbox
+	c_frame_toggle.base.x1 = 50;
+	c_frame_toggle.base.x2 = 50+16;
+	c_frame_toggle.base.y1 = 5;
+	c_frame_toggle.base.y2 = 5+16;
+	c_frame_toggle.checked = frame_visible;
+	c_frame_toggle.fgcolor = CHECKBOX_WIN_FG_COLOR;
+	c_frame_toggle.callback = c_frame_toggle_cb;
+	gui_checkbox_add(&c_frame_toggle);
+	tft_print_line(73,8,BLACK,TRANSPARENT,0,"Show Video");
+
+
+	//Area to select a "color region"
+	a_area.hookedActions =  PEN_DOWN | PEN_UP;
+	a_area.x1 = FRAME_START_X;
+	a_area.y1 = FRAME_START_Y;
+	a_area.x2 = FRAME_END_X;
+	a_area.y2 = FRAME_END_Y;
+	a_area.callback = touchCB;
+    //Do not register it here, we do that later
+
+	if(tracking_current==NULL) {
+		state = error;
+	} else {
+		state = detecting; //Start with the detecting state
+	}
+}
+
+//Callback for when the screen is left/unloaded
+static void leave(void* screen) {
+	//Remove buttons and checkbox
+	gui_button_remove(&b_back);
+	gui_button_remove(&b_select);
+	gui_checkbox_remove(&c_frame_toggle);
+
+	if(state==selecting) { //the user left the screen in the "selecting" phase
+		touch_unregister_area(&a_area); //remove the touch area
+	}
+
+	if(state==tracking) { //the user left the screen in the "tracking" phase
+		tracking_current->stop(tracking_current); //stop tracking
+		pixy_led_set_RGB(0,0,0);
+	}
+}
+
+//Callback for when the screen should be updated
+//This is the main loop of the screen. This method will be called repeatedly
+static void update(void* screen) {
+	switch(state) {
+		case detecting: //Detecting State: Where we try to connect to the pixy
+			if(pixy_init()==0) { //Pixy connection ok
+				state = init; //Go to next state
+			}
+			break;
+
+		case init: //Init State: Where we start the tracking
+			tracking_current->start(tracking_current);
+			state=tracking;
+		break;
+
+		case tracking: //Tracking state: Where we render the frame and the tracked objects
+			pixy_service(); //Receive events (e.g. block-data) from pixy
+
+			if(pixy_blocks_are_new()) { //There are new blocks available
+				if(frame_visible) { //If the user want's us to draw the video data
+					pixy_render_full_frame(FRAME_START_X,FRAME_START_Y);
+				} else { //the user want's a colored background
+					tft_fill_rectangle(FRAME_START_X,FRAME_START_Y,FRAME_END_X,FRAME_END_Y,RGB(200,200,200));
+				}
+
+				#define BLOCK_BUFFER_SIZE 5 //The maximum amount of blocks that we want to receive
+				struct Block blocks[BLOCK_BUFFER_SIZE]; //Storage to receive blocks from pixy
+				int blocks_received= pixy_get_blocks(BLOCK_BUFFER_SIZE,blocks); //Try to receive up to BLOCK_BUFFER_SIZE Blocks from pixy
+
+				if(blocks_received>=0) { //block receiving ok
+					tracking_current->update(tracking_current,blocks,blocks_received); //apply tracking
+
+					//Draw blocks
+					for(int i=0; i<blocks_received; i++) { //for each received block
+						struct Block* block = &(blocks[i]);
+						//block.x and block.y are the center coordinates of the object relative to the camera origin.
+						uint16_t x = block->x-1+FRAME_START_X -block->width/2; //Calculate x-Coordinate on the display
+						uint16_t y = block->y-1+FRAME_START_Y -block->height/2; //Calculate y-Coordinate on the display
+						tft_draw_rectangle(x,y,x+block->width-1, y+block->height-1,WHITE); //Draw a white rectangle
+					}
+				}
+			}
+			break;
+
+		case preselecting: //Pre-Selecting State: Where we set up the color region selection
+		{
+			tracking_current->stop(tracking_current); //Stop tracking
+
+			pixy_render_full_frame(FRAME_START_X,FRAME_START_Y); //Render one frame
+
+			touch_register_area(&a_area); //Register touch area and receive events from now on
+			point1_valid=false; //we start with an invalid point1
+
+			b_select.text="Abort"; //Change the button text to "Abort"
+			gui_button_redraw(&b_select); //redraw button
+
+			state = selecting; //The user can now select a region
+		}
+		break;
+
+		case selected: //Selected State: Where we send the users selection to pixy
+		{
+			//Ensure that (x1,y1) represent the top-left point and (x2,y2) the bottom-right.
+			unsigned int tmp;
+			if(point1.x > point2.x){
+				tmp = point1.x;
+				point1.x = point2.x;
+				point2.x = tmp;
+			}
+
+			if(point1.y > point2.y){
+				tmp = point1.y;
+				point1.y = point2.y;
+				point2.y = tmp;
+			}
+			//Send pixy the selected region
+			pixy_cc_set_region(1,point1.x,point1.y,point2.x-point1.x,point2.y-point1.y);
+		}
+		//no break here: We want the following code to be executed as well
+
+		case abortselecting: //Abort-Selecting State: Where we deinitialize the stuff we used for region selection
+		{
+			touch_unregister_area(&a_area); //Remove the touch area. We'll no longer receive touch events
+
+			b_select.text="Select Color"; //Change the button text back to "Select Color"
+			gui_button_redraw(&b_select); //redraw button
+
+			tracking_current->start(tracking_current); //Start tracking again
+			state=tracking;
+		}
+		break;
+
+		case selecting: //Selecting State: Where we wait on the user to select a color region
+			pixy_service(); //receive pixy events
+			//wait on user to select the image area
+			break;
+
+		case error: //Error State: Where we show an error message and leave the user no other choice than to click the backbutton
+			//wait on user to click the back button
+			break;
+	}
+}
+
+//Declare screen callbacks
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_tracking() {
+	return &screen;
+}

common/app/screen_tracking.h

@@ -0,0 +1,37 @@
+#include "screen.h"
+
+/**
+ * @addtogroup screens
+ */
+/*@{*/
+
+/**
+ * @defgroup tracking Tracking (Screen)
+ * The Tracking-Screen shows the object-tracking and allows some configuration
+ */
+/*@{*/
+
+
+/**
+ * Enum which contains the available tracking implementations
+ */
+enum Tracking_Implementation {
+	OUR_TRACKING,     //!< Our own tracking PID implementation
+	REFERENCE_TRACKING//!< Pixy's internal tracking implementation
+};
+
+/**
+ * Sets the current Mode/Tracking Implementation. Call this before using the screen obtained by get_screen_tracking()
+ * @param impl The new mode
+ */
+void tracking_set_mode(enum Tracking_Implementation impl);
+
+/**
+ * Returns a pointer to the tracking screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_tracking();
+
+/*@}*/
+/*@}*/

common/filesystem/filesystem.c

@@ -0,0 +1,38 @@
+#include "filesystem.h"
+#include "ll_filesystem.h"
+
+bool filesystem_init() {
+	return ll_filesystem_init();
+}
+
+DIRECTORY_STRUCT* filesystem_dir_open(const char* path) {
+	return ll_filesystem_dir_open(path);
+}
+
+void filesystem_dir_close(DIRECTORY_STRUCT* dir) {
+	ll_filesystem_dir_close(dir);
+}
+
+FILE_HANDLE* filesystem_file_open(const char* filename) {
+	return ll_filesystem_file_open(filename);
+}
+
+void filesystem_file_close(FILE_HANDLE* handle) {
+	ll_filesystem_file_close(handle);
+}
+
+FILE_STATUS filesystem_file_seek(FILE_HANDLE* handle, uint32_t offset) {
+	return ll_filesystem_file_seek(handle,offset);
+}
+
+FILE_STATUS filesystem_file_read(FILE_HANDLE* handle, uint8_t* buf, uint32_t size) {
+	return ll_filesystem_file_read(handle,buf,size);
+}
+
+FILE_STATUS filesystem_file_write(FILE_HANDLE* handle, uint8_t* buf, uint32_t size) {
+	return ll_filesystem_file_write(handle,buf,size);
+}
+
+
+
+

common/filesystem/filesystem.h

@@ -0,0 +1,147 @@
+#ifndef FILESYSTEM_H
+#define FILESYSTEM_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @defgroup filesystem Filesystem
+ * The Filesystem Module provides access to files and directories of a the native filesystem.
+ */
+/*@{*/
+
+/**
+ * File Attributes used by implementation
+ * See http://en.wikipedia.org/wiki/Design_of_the_FAT_file_system#attributes for detailed description
+ */
+typedef enum {
+	F_RDO=0x01,//!< File is readonly. You cannot write to it
+	F_HID=0x02,//!< File is hidden
+	F_SYS=0x04,//!< File is a system file
+	F_DIR=0x10,//!< It's a directory and not a file
+	F_ARC=0x20 //!< File has the archive flag set (probably unused)
+} FILE_ATTRIBUTES;
+
+/**
+ * Structure which represents last modified date of a file / directory
+ */
+typedef struct {
+	unsigned year : 7; //!< year from 1980 (0..127)
+	unsigned month: 4; //!< month (1..12)
+	unsigned day: 5; //!< day (1..31)
+} FILE_DATE_STRUCT;
+
+/**
+ * Structure which represents last modified time of a file / directory
+ */
+typedef struct {
+	unsigned hour : 5; //!< hour (0..23)
+	unsigned min: 6; //!< minute (0..59
+	unsigned sec: 5; //!< second/2 (0..29)
+} FILE_TIME_STRUCT;
+
+/**
+ * Structure which represents a file/directory entry. \sa DIRECTORY_STRUCT
+ */
+typedef struct {
+	uint32_t fsize; //!< File size in bytes. 0 for directories
+	FILE_DATE_STRUCT fdate; //!< Last modified date
+	FILE_TIME_STRUCT ftime; //!< Last modified time
+	uint8_t fattrib; //!< File/Directory Attributes
+	char*    fname; //!< File/Directory name
+} FILE_STRUCT;
+
+/**
+ * Structure which represents an open directory with all it's entries. \sa filesystem_dir_open
+ */
+typedef struct {
+	const char* path; //!< Directory path (absolute)
+	uint16_t num_files; //!< Number of files/directories in this directory
+	FILE_STRUCT* files; //!< An array with \ref num_files FILE_STRUCT entries
+} DIRECTORY_STRUCT;
+
+/**
+ * Structure which represents an open file. \sa filesystem_file_open
+ */
+typedef struct {
+	const char* fname; //!< The absolute file name
+	uint32_t fpos; //!< The current byte-position in the file. \sa filesystem_file_seek
+    uint32_t fsize; //!< The total file size in bytes
+} FILE_HANDLE;
+
+/**
+ * Enum to represent the success or error-code of the filesystem_file_* functions
+ */
+typedef enum {
+	F_OK,          //!< Everything ok
+	F_EOF,         //!< The write/read operation tried to write/read past the end of the file. This is not a fatal error.
+	F_EACCESS,     //!< The file can not be read/written due to access problems. This is a fatal error.
+	F_INVALIDPARAM,//!< You passed invalid parameters to the function
+	F_DISKERROR    //!< A lowlevel disk-error occoured. This is a fatal error.
+} FILE_STATUS;
+
+/**
+ * Initializes the filesystem.
+ * Call this method before using any filesystem_* functions
+ * @return true on success
+ */
+bool filesystem_init();
+
+/**
+ * Opens a directory and returns a structure which contains all files/subdirectories. \sa filesystem_dir_close()
+ * @param path The absolute path to the directory to open/read
+ * @return A Pointer to an initialized DIRECTORY_STRUCT on success, NULL on error
+ */
+DIRECTORY_STRUCT* filesystem_dir_open(const char* path);
+
+/**
+ * Closes a previously opened directory. Free's all allocated resources.
+ * @param dir A Pointer to a DIRECTORY_STRUCT obtained by filesystem_dir_open().
+ */
+void filesystem_dir_close(DIRECTORY_STRUCT* dir);
+
+/**
+ * Opens a file for read/writing. \note Depending on the implementation you may only open one file at a time
+ * @param filename The absolute file path
+ * @return A Pointer to a FILE_HANDLE on success, NULL on error.
+ */
+FILE_HANDLE* filesystem_file_open(const char* filename);
+
+/**
+ * Closes a file.
+ * @param handle The FILE_HANDLE obtained by filesystem_file_open()
+ */
+void filesystem_file_close(FILE_HANDLE* handle);
+
+/**
+ * Set's the read/write position to a new position
+ * @param handle The FILE_HANDLE obtained by filesystem_file_open()
+ * @param offset The new read/write position in bytes (absolute).
+ * @return \ref F_OK on success, an error Code otherwise.
+ */
+FILE_STATUS filesystem_file_seek(FILE_HANDLE* handle, uint32_t offset);
+
+/**
+ * Reads some bytes from an open file.
+ * @param handle The FILE_HANDLE obtained by filesystem_file_open()
+ * @param buf The Buffer to write the bytes to
+ * @param size The number of bytes to read
+ * @return \ref F_OK on success, \ref F_EOF if less than \p size bytes could be read, an error Code otherwise.
+ */
+FILE_STATUS filesystem_file_read(FILE_HANDLE* handle, uint8_t* buf, uint32_t size);
+
+/**
+ * Writes some bytes to a open file.
+ * \note Depending on the implementation the file may not be shrinked or expanded.
+ * @param handle The FILE_HANDLE obtained by filesystem_file_open()
+ * @param buf The Buffer to take the bytes from
+ * @param size The number of bytes to write
+ * @return \ref F_OK on success, \ref F_EOF if less than \p size bytes could be written, an error Code otherwise.
+ */
+FILE_STATUS filesystem_file_write(FILE_HANDLE* handle, uint8_t* buf, uint32_t size);
+
+
+/*@}*/
+
+#endif /* FILESYSTEM_H */
+

common/gui/button.c

@@ -0,0 +1,154 @@
+#include "tft.h"
+#include "touch.h"
+#include "button.h"
+#include <string.h>
+
+/* The Idea is as follows:
+ * When the user add's a button we create a touch area for that region and wait for PEN_DOWN events.
+ * Once the user puts the pen down in this area we'll redraw the button with different shadows (feedback)
+ * and we'll now wait on PEN_UP or PEN_LEAVE events.
+ * If the user takes the pen away while in the area (PEN_UP), we call the provided user callback
+ * Otherwise (PEN_LEAVE) we only restore the initial shadows
+ */
+
+/* Possible improvements:
+ * Move the button by 1 pixel while he is pressed, to create a "full 3d" experience
+ * Add events for the case when the button is pressed for a long time, without release
+ */
+
+//Method to calculate the shadow colors used to create the "3d" effect
+void calculate_shadows(uint16_t bgcolor, uint16_t* light_shadow, uint16_t* dark_shadow) {
+	#define BRIGHTNESS_VAL 3 //How much the Brightness is in/decreased for button shadows (3 -> Add/Subtract 1/3 off Full Value)
+
+	uint16_t c_light,c_dark; //c_light and c_dark will be filled with a lighter and a darker color as the background color (for the shadows)
+	uint8_t r,g,b;
+
+	//separate the channels of the 16-bit rgb565 color
+	r=(bgcolor&0xF800)>>11;
+	g=(bgcolor&0x07E0)>>5;
+	b=(bgcolor&0x001F)>>0;
+
+	//For the light shadow color:
+	if((r + 0x1F/BRIGHTNESS_VAL)  > 0x1F) //Adding one third would exceed the maximum of the red channel
+		c_light=0xF800; //Use full red
+	else //adding one third to the red channel is fine
+		c_light=(r+0x1F/BRIGHTNESS_VAL)<<11; //Use same red as in the background, but add one third
+	if((g + 0x3F/BRIGHTNESS_VAL)  > 0x3F) //same for the green channel
+		c_light|=0x07E0;
+	else
+		c_light|=(g+0x3F/BRIGHTNESS_VAL)<<5;
+	if((b + 0x1F/BRIGHTNESS_VAL)  > 0x1F) //and the blue channel
+		c_light|=0x0018;
+	else
+		c_light|=(b+0x1F/BRIGHTNESS_VAL)<<0;
+
+	//For the dark shadow color
+	if(r  > (0x1F/BRIGHTNESS_VAL)) //Subtracting one third would NOT exceed the minimum of the red channel
+		c_dark=(r-0x1F/BRIGHTNESS_VAL)<<11; //Use same red as in the background, but subtract one third
+	else //Subtracting one third would give us a number below zero
+		c_dark=0x0000; //use no red channel
+	if(g > (0x3F/BRIGHTNESS_VAL)) //Same for the green channel
+		c_dark|=(g-0x3F/BRIGHTNESS_VAL)<<5;
+	if(b > (0x1F/BRIGHTNESS_VAL)) //and the blue channel
+		c_dark|=(b-0x1F/BRIGHTNESS_VAL)<<0;
+
+	//Assign the calculated shadows to out parameters
+	if(light_shadow!=NULL) *light_shadow = c_light;
+	if(dark_shadow!=NULL) *dark_shadow = c_dark;
+
+}
+
+//Callback which is called when the user touches the touch-area we created for the button
+void buttons_cb(void* touchArea, TOUCH_ACTION triggeredAction)
+{
+	TOUCH_AREA_STRUCT * area = (TOUCH_AREA_STRUCT*)touchArea;
+	BUTTON_STRUCT* button = (BUTTON_STRUCT*)touchArea; 
+
+	uint16_t c_light,c_dark; //c_light and c_dark will be filled with a lighter and a darker color as the background color (for the shadows)
+	calculate_shadows(button->bgcolor,&c_light,&c_dark);
+
+	switch(triggeredAction)
+	{
+		case PEN_DOWN: //If the user touches the area for the "first time"
+			area->hookedActions=PEN_UP|PEN_LEAVE; //for the future we only want PEN_UP and PEN_LEAVE events
+
+			//Draw shadows
+			tft_draw_line(button->base.x1+1,button->base.y1,button->base.x2-1,button->base.y1,c_dark); //North
+			tft_draw_line(button->base.x1,button->base.y1+1,button->base.x1,button->base.y2-1,c_dark);//West
+			tft_draw_line(button->base.x1+1,button->base.y2,button->base.x2-1,button->base.y2,c_light); //South
+			tft_draw_line(button->base.x2,button->base.y1+1,button->base.x2,button->base.y2-1,c_light);  //East
+		break;
+		case PEN_UP: //If the user took the pen away, while in the area (=button pressed!)
+		case PEN_LEAVE: //or the user "slided out" of the area
+			area->hookedActions=PEN_DOWN; //for the future we only want PEN_DOWN events
+
+			//Draw inverse shadows
+			tft_draw_line(button->base.x1+1,button->base.y1,button->base.x2-1,button->base.y1,c_light); //North
+			tft_draw_line(button->base.x1,button->base.y1+1,button->base.x1,button->base.y2-1,c_light);//West
+			tft_draw_line(button->base.x1+1,button->base.y2,button->base.x2-1,button->base.y2,c_dark); //South
+			tft_draw_line(button->base.x2,button->base.y1+1,button->base.x2,button->base.y2-1,c_dark);  //East
+
+			if(triggeredAction==PEN_UP && button->callback!=NULL) //If the button got "pressed" instead of left, and the user provided a callback
+				button->callback(button); //execute the user callback
+		break;
+		default:break;
+	}
+}
+
+	
+bool gui_button_add(BUTTON_STRUCT* button)
+{
+	if(touch_have_empty(1)) //Check if the touch module can handle one additional area
+	{
+		//Calculate width and height of the button text
+		unsigned int strwidth=tft_font_width(button->font)*strlen(button->text);
+		unsigned char strheight=tft_font_height(button->font);
+
+		button->base.hookedActions=PEN_DOWN; //At first we are interested in PEN_DOWN events
+		button->base.callback = buttons_cb; //Use our own callback for the touch area events
+
+		if(button->base.x2==AUTO) { //The user wants us to calculate the button width automatically
+			//Use string width + half of a character width as button width
+			button->base.x2= button->base.x1 -1 + strwidth+(tft_font_width(button->font)/2);
+		} else if((button->base.x2-button->base.x1+1)<(strwidth+2)) { //the provided width is too small to fit the entire text
+			return false; //report error
+		}
+	
+		if(button->base.y2==AUTO) { //The user wants us to calculate the button height automatically
+			//Use one and a half character heights as button height
+			button->base.y2=button->base.y1 -1 +strheight+(strheight/2);
+		} else if((button->base.y2-button->base.y1+1)<(strheight+2)) { //the provided height is too small to fit the text
+			return  false;
+		}
+		gui_button_redraw(button); //call the redraw method, which will take care of drawing the entire button
+		return touch_register_area(&button->base);	//Register the touch area and receive events for this button, from now on
+	}
+
+	return false; //no more touch areas left
+}
+
+void gui_button_redraw(BUTTON_STRUCT* button)
+{
+	//Calculate text dimensions and shadow colors
+	unsigned int strwidth=tft_font_width(button->font)*strlen(button->text);
+	unsigned char strheight=tft_font_height(button->font);
+	uint16_t c_light,c_dark;
+	calculate_shadows(button->bgcolor,&c_light,&c_dark);
+
+	//Draw the background and the 4 lines (shadow colors)
+	tft_fill_rectangle(button->base.x1+1,button->base.y1+1,button->base.x2-1,button->base.y2-1,button->bgcolor);
+	tft_draw_line(button->base.x1+1,button->base.y1,button->base.x2-1,button->base.y1,c_light); //North
+	tft_draw_line(button->base.x1,button->base.y1+1,button->base.x1,button->base.y2-1,c_light);//West
+	tft_draw_line(button->base.x1+1,button->base.y2,button->base.x2-1,button->base.y2,c_dark); //South
+	tft_draw_line(button->base.x2,button->base.y1+1,button->base.x2,button->base.y2-1,c_dark);  //East
+
+	//Draw the text
+	tft_print_line(button->base.x1+(button->base.x2-button->base.x1+1-strwidth)/2,button->base.y1+(button->base.y2-button->base.y1+1-strheight)/2,button->txtcolor,button->bgcolor,button->font,button->text);	
+		
+}
+
+void gui_button_remove(BUTTON_STRUCT* button)
+{
+	//We only need to unregister the touch area, as we have not allocated anything else
+	touch_unregister_area((TOUCH_AREA_STRUCT*)button);
+}

common/gui/button.h

@@ -0,0 +1,71 @@
+#ifndef BUTTON_H
+#define BUTTON_H
+
+#include "touch.h"
+
+/**
+ * @defgroup gui Gui
+ * The Gui Module
+ */
+/*@{*/
+
+/**
+ * @defgroup button Button
+ * The Button Gui-Element is a clickable, rectangular box with a label inside.
+ * When it is pressed and released you will be notified via the provided callback.
+ */
+
+/*@}*/
+
+/**
+ * @addtogroup button
+ */
+/*@{*/
+
+
+/**
+ * Prototype for Event Listeners (called when the button is pressed)
+ * \note You should NOT execute long running things in this callback nor should you update the gui. But you can call gui_screen_navigate() for instance.
+ * @param button The pointer to the BUTTON_STRUCT where to corresponding Button was pressed
+ */
+typedef void (*BUTTON_CALLBACK)(void *button);
+
+
+/**
+ * Structure to configure the Button
+ */
+typedef struct {
+	TOUCH_AREA_STRUCT base; //!< Basic geometry of the button. You only need to set the x1, y1, x2, y2 members of this struct.
+	uint16_t bgcolor; //!< The 16-bit background color of the button
+	BUTTON_CALLBACK callback; //!< Callback which is executed when the button is pressed
+	uint16_t txtcolor; //!< The 16-bit text color
+	uint8_t font; //!< The number of the font to use
+	const char *text; //!< The label of the button
+} BUTTON_STRUCT;
+
+
+#define AUTO 0  //!< Use this value instead of x2, y2 in the BUTTON_STRUCT to autocalculate the button width/height
+
+/**
+ * Adds a button. Your Callback will be called from now on, if the button was pressed
+ * @param button A Pointer to the preinitialized BUTTON_STRUCT
+ * @return true on success
+ */
+bool gui_button_add(BUTTON_STRUCT* button);
+
+/**
+ * Removes the button. You will no longer receive events for this button. This function will not overdraw the region where the button was located.
+ * @param button A Pointer to the BUTTON_STRUCT
+ */
+void gui_button_remove(BUTTON_STRUCT* button);
+
+/**
+ * Redraws the button. Call this method if you have to redraw the entire screen or if you want to draw a button on top of an image.
+ * @param button A Pointer to the BUTTON_STRUCT
+ */
+void gui_button_redraw(BUTTON_STRUCT* button);
+
+
+/*@}*/
+
+#endif /* BUTTON_H */

common/gui/checkbox.c

@@ -0,0 +1,121 @@
+#include "tft.h"
+#include "touch.h"
+#include "checkbox.h"
+#include <stdio.h>
+
+/* The idea is as follows:
+ * When the user creates a checkbox we create a touch area for that region and wait for PEN_DOWN events.
+ * Once the user puts the pen down in this area we'll redraw the checkbox with different shadows (feedback)
+ * and we'll now wait on PEN_UP or PEN_LEAVE events.
+ * If the user takes the pen away while in the area (PEN_UP), we toggle the checkbox and we call the provided user callback
+ * Otherwise (PEN_LEAVE) we only restore the initial shadows
+ */
+
+
+#define ACTIVE_COLOR RGB(251,208,123) //shadow color (inside of border)
+#define BORDER_COLOR RGB(29,82,129) //1px border color
+#define BACKGROUND_COLOR WHITE //Background color
+
+//Callback which is called when the user touches the touch-area we created for the checkbox
+void checkboxes_cb(void* touchArea, TOUCH_ACTION triggeredAction)
+{
+	TOUCH_AREA_STRUCT * area = (TOUCH_AREA_STRUCT*)touchArea;
+	CHECKBOX_STRUCT* checkbox = (CHECKBOX_STRUCT*)touchArea;
+	switch(triggeredAction)
+	{
+		case PEN_DOWN:  //If the user touches the area for the "first time"
+			area->hookedActions=PEN_UP|PEN_LEAVE; //for the future we only want PEN_UP and PEN_LEAVE events
+
+			//Draw active shadows
+			tft_draw_rectangle(checkbox->base.x1+1,checkbox->base.y1+1,checkbox->base.x2-1,checkbox->base.y2-1,ACTIVE_COLOR);
+			tft_draw_rectangle(checkbox->base.x1+2,checkbox->base.y1+2,checkbox->base.x2-2,checkbox->base.y2-2,ACTIVE_COLOR);
+		break;
+		case PEN_UP: //If the user took the pen away, while in the area (=toggle checkbox!)
+			checkbox->checked=!checkbox->checked; //Toggle checkbox state
+			gui_checkbox_update(checkbox); //redraw/overdraw tickmark
+			if(checkbox->callback!=NULL) { //The user provided a callback
+				checkbox->callback(checkbox,checkbox->checked); //Call the provided callback with the new checked state
+			}
+			// no break statement here!
+		case PEN_LEAVE: //if the user "slided out" of the area
+			area->hookedActions=PEN_DOWN;  //for the future we only want PEN_DOWN events
+
+			//Draw inactive shadows
+			tft_draw_rectangle(checkbox->base.x1+1,checkbox->base.y1+1,checkbox->base.x2-1,checkbox->base.y2-1,BACKGROUND_COLOR);
+			tft_draw_rectangle(checkbox->base.x1+2,checkbox->base.y1+2,checkbox->base.x2-2,checkbox->base.y2-2,BACKGROUND_COLOR);
+		break;
+		default:break;
+	}
+}
+
+bool gui_checkbox_add(CHECKBOX_STRUCT* checkbox)
+{
+	if(touch_have_empty(1)) //Check if the touch module can handle one additional area
+	{
+		unsigned char size=0;
+		checkbox->base.hookedActions=PEN_DOWN; //At first we are interested in PEN_DOWN events
+		checkbox->base.callback = checkboxes_cb; //Use our own callback for the touch area events
+
+		//Check the size of the checkbox
+		if(checkbox->base.x2>checkbox->base.x1)
+			size = checkbox->base.x2 - checkbox->base.x1; //use width a as size
+		if(checkbox->base.y2>checkbox->base.y1)	
+		{
+			if((checkbox->base.y2 - checkbox->base.y1)>size)  //height is larger than size
+				size = checkbox->base.y2 - checkbox->base.y1; //use height as size
+		}
+		if(size==0) { //no size found (maybe swap x2 and x1 or y2 and y1 ?)
+			return false; //signal error
+		}
+		if((size&0x01)) //the size is an odd number
+			size++; //make size an even number
+
+		//Correct x2,y2 so that the checkbox is quadratic
+		checkbox->base.x2 = checkbox->base.x1 + size;
+		checkbox->base.y2 = checkbox->base.y1 + size;
+
+		gui_checkbox_redraw(checkbox);//Call redraw method, which will take care of the drawing of the entire checkbox
+
+		return touch_register_area(&checkbox->base); //Register the touch area and receive events for this checkbox, from now on
+	}
+
+	return false; //no more touch areas left
+}
+ 
+void gui_checkbox_redraw(CHECKBOX_STRUCT* checkbox)
+{
+	//Draw background and border
+	tft_fill_rectangle(checkbox->base.x1+1,checkbox->base.y1+1,checkbox->base.x2-1,checkbox->base.y2-1,BACKGROUND_COLOR);
+	tft_draw_rectangle(checkbox->base.x1,checkbox->base.y1,checkbox->base.x2,checkbox->base.y2,BORDER_COLOR);
+
+	if(checkbox->checked) { //checkbox is currently checked
+		gui_checkbox_update(checkbox); //Call update method which will draw the tickmark
+	}
+}
+
+void gui_checkbox_remove(CHECKBOX_STRUCT* checkbox)
+{
+	//We only need to unregister the touch area, as we have not allocated anything else
+	 touch_unregister_area((TOUCH_AREA_STRUCT*)checkbox);
+}
+
+void gui_checkbox_update(CHECKBOX_STRUCT* checkbox)
+{
+	unsigned int c = (checkbox->checked)? checkbox->fgcolor:BACKGROUND_COLOR; //color to use for the tickmark
+
+	//helper points inside the checkbox
+	unsigned int xcent = checkbox->base.x1+(checkbox->base.x2-checkbox->base.x1)*6/14;
+	unsigned int yleft = checkbox->base.y2 - (xcent- checkbox->base.x1) - 1 ;
+	unsigned int yright = checkbox->base.y2 - (checkbox->base.x2 - xcent) - 1 ;
+	unsigned int ybot = checkbox->base.y2 - 4;
+
+	//Draw tickmark as a 3pixel wide line
+	tft_draw_line(checkbox->base.x1+3,yleft-1,xcent,ybot -1,c);
+	tft_draw_line(checkbox->base.x1+3,yleft,xcent,ybot ,c);
+	tft_draw_line(checkbox->base.x1+3,yleft+1,xcent,ybot + 1,c);
+	xcent++;
+	ybot--;
+	tft_draw_line(xcent,ybot-1,checkbox->base.x2-3,yright-1,c);
+	tft_draw_line(xcent,ybot,checkbox->base.x2-3,yright+0,c);
+	tft_draw_line(xcent,ybot+1,checkbox->base.x2-3,yright+1,c);
+}

common/gui/checkbox.h

@@ -0,0 +1,70 @@
+#ifndef CHECKBOX_H
+#define CHECKBOX_H
+
+#include "touch.h"
+
+/**
+ * @addtogroup gui
+ */
+/*@{*/
+
+/**
+ * @defgroup checkbox Checkbox
+ * The Checkbox Gui-Element is a clickable, rectangular box with an optional tickmark inside of it.
+ * When the checkbox is pressed and released it's tick state changes and you will be notified via the provided callback.
+ */
+/*@}*/
+
+/**
+ * @addtogroup checkbox
+ */
+/*@{*/
+
+/**
+ * Prototype for Event Listeners (called when the checkbox state has changed)
+ * \note You should NOT execute long running things in this callback nor should you update the gui. But you can call gui_screen_navigate() for instance.
+ * @param checkbox The pointer to the CHECKBOX_STRUCT where to corresponding Checkbox has changed the state
+ * @param checked A boolean which indicates whether the checkbox is now checked or not.
+ */
+typedef void (*CHECKBOX_CALLBACK)(void *checkbox, bool checked);
+
+/**
+ * Structure to configure the Checkbox
+ */
+typedef struct {
+	TOUCH_AREA_STRUCT base; //!<  Basic geometry of the Checkbox. You only need to set the x1, y1, x2, y2 members of this struct.
+	uint16_t fgcolor; //!< The 16-bit color of the tickmark
+	bool checked; //!< A boolean which indicates whether or not the checkbox is currently checked.
+	CHECKBOX_CALLBACK callback; //!< Callback which is executed when the checkbox changes state
+} CHECKBOX_STRUCT;
+
+/**
+ * Adds a checkbox. Your Callback will be called from now on, if the checkbox changes state
+ * @param checkbox A Pointer to the preinitialized CHECKBOX_STRUCT
+ * @return true on success
+ */
+bool gui_checkbox_add(CHECKBOX_STRUCT* checkbox);
+
+/**
+ * Removes the checkbox. You will no longer receive events for this checkbox. This function will not overdraw the region where the checkbox was located.
+ * @param checkbox A Pointer to the CHECKBOX_STRUCT
+ */
+void gui_checkbox_remove(CHECKBOX_STRUCT* checkbox);
+
+/**
+ * Updates the checkbox. Call this function when you change the state of the checkbox through code.
+ * @param checkbox A Pointer to the CHECKBOX_STRUCT
+ */
+void gui_checkbox_update(CHECKBOX_STRUCT* checkbox);
+
+/**
+ * Redraws the checkbox. Call this method if you have to redraw the entire screen or if you want to draw a checkbox on top of an image.
+ * @param checkbox A Pointer to the CHECKBOX_STRUCT
+ */
+void gui_checkbox_redraw(CHECKBOX_STRUCT* checkbox);
+
+#define CHECKBOX_WIN_FG_COLOR RGB(32,161,34)
+
+/*@}*/
+
+#endif /* CHECKBOX_H */

common/gui/numupdown.c

@@ -0,0 +1,143 @@
+#include "tft.h"
+#include "touch.h"
+#include "button.h"
+#include "numupdown.h"
+#include <stdio.h> //for sprintf
+#include <stddef.h> //for offsetof macro
+#include <stdlib.h> //for abs
+
+/* The idea is as follows:
+ * When the user add's a numupdown we create two buttons, one with a plus and one with a minus sign in it
+ * When the user presses one of the buttons we check and increase the value and execute the provided user callback
+ */
+
+
+#define BASE_COLOR RGB(90,90,90) //Background color for the whole element
+
+//Callback which is called when the user presses the "plus" button
+void button_up_cb(void* button)
+{
+	//Get the pointer to the numupdown: subtract the offset of the buttonUp member in the struct from the button pointer
+	NUMUPDOWN_STRUCT* element = button-offsetof(NUMUPDOWN_STRUCT,buttonUp);
+
+	if(element->value<element->max) { //old value lies below the maximum
+		element->value++; //let's increase the value
+		gui_numupdown_update(element); //and redraw everything
+		if(element->callback!=NULL) { //the user provided a callback
+			element->callback(element,element->value); //Call the user callback with the new value
+		}
+	}
+}
+
+//Callback which is called when the user presses the "minus" button
+void button_down_cb(void* button)
+{
+	//Get the pointer to the numupdown: subtract the offset of the buttonDown member in the struct from the button pointer
+	NUMUPDOWN_STRUCT* element = button-offsetof(NUMUPDOWN_STRUCT,buttonDown);
+
+	if(element->value>element->min) { //old value lies above the minimum
+		element->value--; //let's decrease the value
+		gui_numupdown_update(element); //and redraw everything
+		if(element->callback!=NULL) { //the user provided a callback
+			element->callback(element,element->value); //Call the user callback with the new value
+		}
+	}
+}
+
+//Method to calculate the number of characters needed to print the provided number in decimal notation (with optional sign)
+static uint8_t calc_text_width(int16_t val) {
+	uint8_t width = 1 + (val<0); //1 if positive, 2 if negative (to let space for sign)	
+	val=abs(val); //Make the number positive
+	while(val>=10) { //while we have two or more digits
+		val/=10; //remove one digit
+		width++; //add one character
+	}
+	return width;
+}
+
+
+bool gui_numupdown_add(NUMUPDOWN_STRUCT* numupdown)
+{
+	 if(touch_have_empty(2)) //Check if the touch module can handle two additional areas
+	 {
+		if(numupdown->min > numupdown->max) { //min is bigger than max?
+			return false; //invalid parameter
+		}
+
+		if(numupdown->value < numupdown->min) { //value is smaller than min?
+			numupdown->value = numupdown->min; //normalize value
+		} else if(numupdown->value > numupdown->max) { //value is bigger than max?
+			numupdown->value = numupdown->max;  //normalize value
+		}
+		
+		 uint8_t tw1 = calc_text_width(numupdown->max); //Calculate character width to render maximum value
+		 uint8_t tw2 = calc_text_width(numupdown->min); //Calculate character width to render minimum value
+		 if(tw2 > tw1) tw1 = tw2; //ensure tw1 contains the larger number of the two
+	     uint8_t width= tft_font_width(0)*(tw1+1); //Calculate width of the number area
+
+	     //Add "minus" button to the left side of the number area
+		 numupdown->buttonDown.base.x1=numupdown->x;
+		 numupdown->buttonDown.base.y1=numupdown->y;
+		 numupdown->buttonDown.base.x2=AUTO;
+		 numupdown->buttonDown.base.y2=numupdown->y+tft_font_height(0)*2;
+		 numupdown->buttonDown.text="-";
+		 numupdown->buttonDown.font=0;
+		 numupdown->buttonDown.bgcolor=BASE_COLOR;
+		 numupdown->buttonDown.txtcolor=WHITE;
+		 numupdown->buttonDown.callback = button_down_cb;
+		 gui_button_add(&numupdown->buttonDown);
+
+		 //Add "plus" button to the right side of the number area
+		 numupdown->buttonUp.base.x1=numupdown->buttonDown.base.x2+width+2;
+		 numupdown->buttonUp.base.y1=numupdown->y;
+		 numupdown->buttonUp.base.x2=AUTO;
+		 numupdown->buttonUp.base.y2=numupdown->y +tft_font_height(0)*2;
+		 numupdown->buttonUp.text="+";
+		 numupdown->buttonUp.font=0;
+		 numupdown->buttonUp.bgcolor=BASE_COLOR;
+		 numupdown->buttonUp.txtcolor=WHITE;
+		 numupdown->buttonUp.callback = button_up_cb;
+		 gui_button_add(&numupdown->buttonUp);
+
+		 //Draw background and label of the number area
+		 tft_fill_rectangle(numupdown->buttonDown.base.x2+2,numupdown->y,numupdown->buttonDown.base.x2+width,numupdown->buttonUp.base.y2,BASE_COLOR);
+		 tft_print_formatted(numupdown->buttonDown.base.x2+2+tft_font_width(0)/2,numupdown->y+tft_font_height(0)/2,numupdown->fgcolor,BASE_COLOR,0,"%*d",tw1,numupdown->value);
+		
+		 return true;
+	 }
+
+	 return false; //not enough touch areas left
+ }
+ 
+void gui_numupdown_remove(NUMUPDOWN_STRUCT* numupdown)
+{
+	 //remove the two buttons, we have no other allocated resources
+	 gui_button_remove(&numupdown->buttonUp);
+	 gui_button_remove(&numupdown->buttonDown);
+}
+
+
+void gui_numupdown_redraw(NUMUPDOWN_STRUCT* numupdown)
+{
+	//redraw the two buttons
+	gui_button_redraw(&numupdown->buttonUp);
+	gui_button_redraw(&numupdown->buttonDown);
+
+	//call update method which will take care of the number-area rendering
+	gui_numupdown_update(numupdown);
+}
+
+void gui_numupdown_update(NUMUPDOWN_STRUCT* numupdown)
+{
+	 //Calculate the number area width again (see above)
+	 uint8_t tw1 = calc_text_width(numupdown->max);
+	 uint8_t tw2 = calc_text_width(numupdown->min);
+	 if(tw2 > tw1) tw1 = tw2;
+	 uint8_t width= tft_font_width(0)*(tw1+1);
+
+	 //Draw background and label of the number area
+	 tft_fill_rectangle(numupdown->buttonDown.base.x2+2,numupdown->y,numupdown->buttonDown.base.x2+width,numupdown->buttonUp.base.y2,BASE_COLOR);
+	 tft_print_formatted(numupdown->buttonDown.base.x2+2+tft_font_width(0)/2,numupdown->y+tft_font_height(0)/2,numupdown->fgcolor,BASE_COLOR,0,"%*d",tw1,numupdown->value);
+}
+
+

common/gui/numupdown.h

@@ -0,0 +1,73 @@
+#ifndef NUMUPDOWN_H
+#define NUMUPDOWN_H
+
+#include "button.h"
+
+/**
+ * @addtogroup gui
+ */
+/*@{*/
+
+/**
+ * @defgroup numupdown NummericUpDown
+ * The NummericUpDown Gui Element
+ */
+/*@}*/
+
+/**
+ * @addtogroup numupdown
+ */
+/*@{*/
+
+/**
+ * Prototype for Event Listeners (called when the NummericUpDown value has changed)
+ * \note You should NOT execute long running things in this callback nor should you update the gui. But you can call gui_screen_navigate() for instance.
+ * @param numupdown The pointer to the NUMUPDOWN_STRUCT where to corresponding NummericUpDown has changed it's value
+ * @param value The new value of the NummericUpDown
+ */
+typedef void (*NUMUPDOWN_CALLBACK)(void *numupdown, int16_t value);
+
+/**
+ * Structure to configure the NummericUpDown
+ */
+typedef struct {
+	uint16_t x; //!< The x-Coordinate of the Top-Left Starting Point.
+	uint16_t y; //!< The y-Coordinate of the Top-Left Starting Point.
+	uint16_t fgcolor; //!< The 16-bit color of the value-text
+	int16_t value; //!< The current/default value
+	int16_t min; //!< The minimum possible value (inclusive)
+	int16_t max; //!< The maximum possible value (inclusive)
+	NUMUPDOWN_CALLBACK callback; //!< Callback which is executed when the value changes
+	
+	BUTTON_STRUCT buttonUp; //!< For internal use, don't change, don't initialize
+	BUTTON_STRUCT buttonDown; //!< For internal use, don't change, don't initialize
+} NUMUPDOWN_STRUCT;
+
+/**
+ * Adds a NummericUpDown. Your Callback will be called from now on, if the numupdown's value changes
+ * @param numupdown A Pointer to the preinitialized NUMUPDOWN_STRUCT
+ * @return true on success
+ */
+bool gui_numupdown_add(NUMUPDOWN_STRUCT* numupdown);
+
+/**
+ * Removes the NummericUpDown. You will no longer receive events for this numupdown. This function will not overdraw the region where the numupdown was located.
+ * @param numupdown A Pointer to the NUMUPDOWN_STRUCT
+ */
+void gui_numupdown_remove(NUMUPDOWN_STRUCT* numupdown);
+
+/**
+ * Updates the NummericUpDown. Call this function when you change the value/min/max of the numupdown through code.
+ * @param numupdown A Pointer to the NUMUPDOWN_STRUCT
+ */
+void gui_numupdown_update(NUMUPDOWN_STRUCT* numupdown);
+
+/**
+ * Redraws the NummericUpDown. Call this method if you have to redraw the entire screen or if you want to draw a numupdown on top of an image.
+ * @param numupdown A Pointer to the NUMUPDOWN_STRUCT
+ */
+void gui_numupdown_redraw(NUMUPDOWN_STRUCT* numupdown);
+
+/*@}*/
+
+#endif /* NUMUPDOWN_H */

common/gui/screen.c

@@ -0,0 +1,75 @@
+#include "screen.h"
+
+/* The idea is as follows:
+ * We only call screen callbacks from the gui_screen_update() method, which is called from the applications main loop.
+ * Instructions to switch the screen will be delayed until the gui_screen_update() method is called again.
+ * This makes it safe to change the screen from an touch interrupt (e.g. button callback)
+ */
+
+/* Possible Improvements:
+ * Ensure that you can not navigate to a screen which is already in the history (because it will corrupt the list)
+ */
+
+static SCREEN_STRUCT* screen_list = NULL; //Head of the linked list which stores the screen history.
+static SCREEN_STRUCT* screen_current = NULL; //Pointer to the current screen (= tail of the list)
+static volatile SCREEN_STRUCT* screen_goto = NULL; //Screen we should navigate to once we enter the gui_screen_update() method again
+
+SCREEN_STRUCT* gui_screen_get_current() {
+	return screen_current;
+}
+
+
+void gui_screen_update() {
+  	if(screen_goto!=NULL) { //we received the task to switch the screen
+		SCREEN_STRUCT* go = (SCREEN_STRUCT*) screen_goto; //Backup volatile variable
+		screen_goto=NULL; //reset the "goto instruction", since we're processing it now
+		if(go->next!=NULL) { //The screen is not the last in the list, so we're going back
+		    if(go->next!=screen_current) { //this condition should always be false
+		    	return; //list corrupted?
+		    }
+			screen_current->on_leave(screen_current); //let the current screen free/unregister it's resources
+       		go->next=NULL; //remove the current screen from the list
+		} else { //we're going forward (to a new screen)
+			if(screen_current!=NULL) { //this is not the first screen
+					screen_current->on_leave(screen_current); //let the current screen free/unregister it's resources
+					screen_current->next = go; //append the new screen to the end of the list
+			} else { //first screen ever seen
+					screen_list=go; //set the new screen as list-head
+			}
+		}
+		go->on_enter(go); //let the new screen allocate/register it's resources
+		screen_current = go; //the new screen is now the current screen. Transition done
+	}		
+
+	if(screen_current!=NULL) { //A screen has been set
+		screen_current->on_update(screen_current); //Update current screen	
+	}
+}
+
+
+
+bool gui_screen_navigate(SCREEN_STRUCT* screen) {
+	if(screen==NULL || screen==screen_current || screen==screen_goto) { //invalid argument passed
+		return false;
+	}
+	screen->next = NULL; //this will become the new tail of the list, so the next pointer must be NULL
+	screen_goto=screen; //"send message" to main loop, to switch the screen
+	return true;
+}
+
+bool gui_screen_back() {
+	if(screen_list==NULL) { //the list head is emtpy, nothing to go back to
+		return false;
+	}
+	SCREEN_STRUCT* current = screen_list;
+	SCREEN_STRUCT* last = NULL;
+	//Find second last element in list
+	while(current->next != NULL) {
+		last = current;
+		current = current->next;
+	}
+	if(last==NULL) return false; //There's only a single screen, there's no going back here
+	if(current!=screen_current) return false; //The last entry in the list is not the current screen. List corrupted?
+	screen_goto=last; //"send message" to main loop, to switch the screen
+	return true;	
+}

common/gui/screen.h

@@ -0,0 +1,76 @@
+#ifndef SCREEN_H
+#define SCREEN_H
+
+#include <stdio.h>
+#include <stdbool.h>
+
+/**
+ * @addtogroup gui
+ */
+/*@{*/
+
+/**
+ * @defgroup screen Screen
+ * The Screen Submodule provides an api to navigate between different "screens" on the UI.
+ * Each screen must provide an enter, update and a leave method; which will be called from this module at the right time. 
+ * The implemented screens of the application are documented in the \ref screens module.
+ */
+/*@}*/
+
+/**
+ * @addtogroup screen
+ */
+/*@{*/
+
+/**
+ * Prototype for Event Listeners (called when the screen is entered, left or should be updated)
+ * @param screen The pointer to the SCREEN_STRUCT where the event occurred
+ */
+typedef void (*SCREEN_CALLBACK)(void* screen);
+
+/**
+ * Structure to configure the Screen
+ */
+typedef struct SCREEN_S{
+	SCREEN_CALLBACK on_enter; //!< The Callback which is called when the screen is entered. Add/Register all UI-Elements here
+	SCREEN_CALLBACK on_leave; //!< The Callback which is called when the screen is left. Remove/Unregister all UI-Elements here
+	SCREEN_CALLBACK on_update; //!< The Callback which is called repeatedly when the screen should be updated. Update/Redraw all UI-Elements here
+
+	struct SCREEN_S* next; //!< Used internally. do not modify, do not initialize
+} SCREEN_STRUCT;
+
+
+
+/**
+ * Navigate to the given screen as soon as the app enters the main loop again (and gui_screen_update() is called)
+ * It's safe to call this method from an interrupt
+ * @note Do not pass a screen which is already in your history of screens!
+ * @param screen A Pointer to the preinitialized SCREEN_STRUCT
+ * @return true on success
+ */
+bool gui_screen_navigate(SCREEN_STRUCT* screen);
+
+/**
+ * Navigate one screen back as soon as the app enters the main loop again.
+ * It's safe to call this method from an interrupt
+ * @return true on success
+ */
+bool gui_screen_back();
+
+
+/**
+ * Returns the currently active screen
+ * @return A Pointer to the active SCREEN_STRUCT
+ */
+SCREEN_STRUCT* gui_screen_get_current();
+
+//Updates/switches the screens. Call this from the app main loop, as fast as you can.
+/**
+ * Updates the current screen. Switches the screen if gui_screen_navigate() or gui_screen_back() have been called since the last call to this method.
+ * This method should be called repeatedly from the main loop (e.g. app_process())
+ */
+void gui_screen_update();
+
+/*@}*/
+
+#endif /* SCREEN_H */

common/lowlevel/ll_filesystem.h

@@ -0,0 +1,30 @@
+#include "filesystem.h"
+
+/**
+ * @addtogroup lowlevel
+ */
+/*@{*/
+
+/**
+ * @defgroup ll_filesystem Filesystem (LowLevel)
+ * Low level functions for the \ref filesystem module
+ */
+/*@}*/
+
+/**
+ * @addtogroup ll_filesystem
+ */
+/*@{*/
+
+
+bool ll_filesystem_init();
+
+DIRECTORY_STRUCT* ll_filesystem_dir_open(const char* path);
+void ll_filesystem_dir_close(DIRECTORY_STRUCT* dir);
+FILE_HANDLE* ll_filesystem_file_open(const char* filename);
+void ll_filesystem_file_close(FILE_HANDLE* handle);
+FILE_STATUS ll_filesystem_file_seek(FILE_HANDLE* handle, uint32_t offset);
+FILE_STATUS ll_filesystem_file_read(FILE_HANDLE* handle, uint8_t* buf, uint32_t size);
+FILE_STATUS ll_filesystem_file_write(FILE_HANDLE* handle, uint8_t* buf, uint32_t size);
+
+/*@}*/

common/lowlevel/ll_system.h

@@ -0,0 +1,29 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+
+/**
+ * @defgroup lowlevel LowLevel
+ * The Low-Level platform abstraction layer
+ */
+/*@{*/
+
+/**
+ * @defgroup ll_system System (LowLevel)
+ * Low level functions of the \ref system Module
+ */
+
+/*@}*/
+
+/**
+ * @addtogroup ll_system
+ */
+/*@{*/
+
+
+bool ll_system_init();
+void ll_system_delay(uint32_t msec);
+void ll_system_process();
+void ll_system_toggle_led();
+
+/*@}*/

common/lowlevel/ll_tft.h

@@ -0,0 +1,41 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+
+/**
+ * @addtogroup lowlevel
+ */
+/*@{*/
+
+/**
+ * @defgroup ll_tft TFT (LowLevel)
+ * Low level functions for the \ref tft module
+ */
+/*@}*/
+
+/**
+ * @addtogroup ll_tft
+ */
+/*@{*/
+
+// init functions
+bool ll_tft_init();
+
+// draw functions
+void ll_tft_clear(uint16_t color);
+void ll_tft_draw_line(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color);
+void ll_tft_draw_pixel(uint16_t x,uint16_t y,uint16_t color);
+void ll_tft_draw_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color);
+void ll_tft_fill_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color);
+void ll_tft_draw_bitmap_unscaled(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint16_t *dat);
+void ll_tft_draw_circle(uint16_t x, uint16_t y, uint16_t r, uint16_t color);
+
+
+uint8_t ll_tft_num_fonts();
+uint8_t ll_tft_font_height(uint8_t fontnum);
+uint8_t ll_tft_font_width(uint8_t fontnum);
+void ll_tft_draw_char(uint16_t x, uint16_t y, uint16_t color, uint16_t bgcolor, uint8_t font, char c);
+
+/*@}*/
+
+

common/lowlevel/ll_touch.h

@@ -0,0 +1,23 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * @addtogroup lowlevel
+ */
+/*@{*/
+
+/**
+ * @defgroup ll_touch Touch (LowLevel)
+ * Low level functions for the \ref touch module
+ */
+/*@}*/
+
+/**
+ * @addtogroup ll_touch
+ */
+/*@{*/
+
+
+bool ll_touch_init();
+
+/*@}*/

common/pixy/pixy.h

@@ -0,0 +1,294 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef __PIXY_H__
+#define __PIXY_H__
+
+#include <stdint.h>
+#include "pixydefs.h"
+
+// Pixy C API //
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+  /**
+   * @defgroup pixy Pixy
+   * The Pixy Module
+   */
+  /*@{*/
+
+
+  #define PIXY_MAX_SIGNATURE          7
+
+  // Pixy x-y position values
+  #define PIXY_MIN_X                  0
+  #define PIXY_MAX_X                  319
+  #define PIXY_MIN_Y                  0
+  #define PIXY_MAX_Y                  199
+
+  // RC-servo values
+  #define PIXY_RCS_MIN_POS            0
+  #define PIXY_RCS_MAX_POS            1000
+  #define PIXY_RCS_CENTER_POS         ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2)
+
+  // Block types
+  #define PIXY_BLOCKTYPE_NORMAL       0
+  #define PIXY_BLOCKTYPE_COLOR_CODE   1
+
+  struct Block
+  {
+    /*void print(char *buf)
+    {
+      int i, j;
+      char sig[6], d;
+      bool flag;
+      if (type==PIXY_BLOCKTYPE_COLOR_CODE)
+      {
+        // convert signature number to an octal string
+        for (i=12, j=0, flag=false; i>=0; i-=3)
+        {
+          d = (signature>>i)&0x07;
+          if (d>0 && !flag)
+            flag = true;
+          if (flag)
+            sig[j++] = d + '0';
+        }
+        sig[j] = '\0';	
+        sprintf(buf, "CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d", sig, signature, x, y, width, height, angle);
+      }
+      else // regular block.  Note, angle is always zero, so no need to print
+        sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d", signature, x, y, width, height);		
+    }*/
+
+    uint16_t type;
+    uint16_t signature;
+    uint16_t x;
+    uint16_t y;
+    uint16_t width;
+    uint16_t height;
+    int16_t  angle;
+  };
+
+  /**
+    @brief Creates a connection with Pixy and listens for Pixy messages.
+    @return  0                         Success
+    @return  PIXY_ERROR_USB_IO         USB Error: I/O
+    @return  PIXY_ERROR_NOT_FOUND      USB Error: Pixy not found
+    @return  PIXY_ERROR_USB_BUSY       USB Error: Busy
+    @return  PIXY_ERROR_USB_NO_DEVICE  USB Error: No device
+  */
+  int pixy_init();
+
+  /**
+    @brief      Indicates when new block data from Pixy is received.
+
+    @return  1  New Data:   Block data has been updated.
+    @return  0  Stale Data: Block data has not changed since pixy_get_blocks() was
+                            last called.
+  */
+  int pixy_blocks_are_new();
+
+  /**
+    @brief      Copies up to 'max_blocks' number of Blocks to the address pointed
+                to by 'blocks'.
+    @param[in]  max_blocks Maximum number of Blocks to copy to the address pointed to
+                           by 'blocks'.
+    @param[out] blocks     Address of an array in which to copy the blocks to.
+                           The array must be large enough to write 'max_blocks' number
+                           of Blocks to.
+    @return  Non-negative                  Success: Number of blocks copied
+    @return  PIXY_ERROR_USB_IO             USB Error: I/O
+    @return  PIXY_ERROR_NOT_FOUND          USB Error: Pixy not found
+    @return  PIXY_ERROR_USB_BUSY           USB Error: Busy
+    @return  PIXY_ERROR_USB_NO_DEVICE      USB Error: No device
+    @return  PIXY_ERROR_INVALID_PARAMETER  Invalid pararmeter specified
+  */
+  int pixy_get_blocks(uint16_t max_blocks, struct Block * blocks);
+
+
+
+  int pixy_service();
+
+  /**
+    @brief      Send a command to Pixy.
+    @param[in]  name  Chirp remote procedure call identifier string.
+    @return     -1    Error
+
+  */
+  int pixy_command(const char *name, ...);
+
+  /**
+    @brief Terminates connection with Pixy.
+  */
+  void pixy_close();
+
+  /**
+    @brief Send description of pixy error to stdout.
+    @param[in] error_code  Pixy error code
+  */
+  void pixy_error(int error_code);
+
+  /**
+    @brief  Set color of pixy LED.
+    @param[in] red   Brightness value for red LED element.   [0, 255] 0 = Off, 255 = On
+    @param[in] green Brightness value for green LED element. [0, 255] 0 = Off, 255 = On
+    @param[in] blue  Brightness value for blue LED element.  [0, 255] 0 = Off, 255 = On
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_led_set_RGB(uint8_t red, uint8_t green, uint8_t blue);
+
+  /**
+    @brief  Set pixy LED maximum current.
+    @param[in] current  Maximum current (microamps).
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_led_set_max_current(uint32_t current);
+
+  /**
+    @brief   Get pixy LED maximum current.
+    @return     Non-negative Maximum LED current value (microamps).
+    @return     Negative     Error
+  */
+  int pixy_led_get_max_current();
+
+  /**
+    @brief    Enable or disable pixy camera auto white balance.
+    @param    value  1: Enable white balance.
+                      0: Disable white balance.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_set_auto_white_balance(uint8_t value);
+
+  /**
+    @brief    Get pixy camera auto white balance setting.
+    @return     1         Auto white balance is enabled.
+    @return     0         Auto white balance is disabled.
+    @return     Negative  Error
+  */
+  int pixy_cam_get_auto_white_balance();
+
+  /**
+    @brief   Get pixy camera white balance()
+    @return  Composite value for RGB white balance:
+             white balance = green_value + (red_value << 8) + (blue << 16)
+  */
+  uint32_t pixy_cam_get_white_balance_value();
+
+  /**
+    @brief     Set pixy camera white balance.
+    @param[in] red    Red white balance value.
+    @param[in] green  Green white balance value.
+    @param[in] blue   Blue white balance value.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_set_white_balance_value(uint8_t red, uint8_t green, uint8_t blue);
+
+  /**
+    @brief     Enable or disable pixy camera auto exposure compensation.
+    @param[in] enable  0: Disable auto exposure compensation.
+                       1: Enable auto exposure compensation.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_set_auto_exposure_compensation(uint8_t enable);
+
+  /**
+    @brief     Get pixy camera auto exposure compensation setting.
+    @return     1         Auto exposure compensation enabled.
+    @return     0         Auto exposure compensation disabled.
+    @return     Negative  Error
+  */
+  int pixy_cam_get_auto_exposure_compensation();
+
+  /**
+    @brief     Set pixy camera exposure compensation.
+    @param[in] gain  Camera gain.
+    @param[in] comp  Camera exposure compensation.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_set_exposure_compensation(uint8_t gain, uint16_t comp);
+
+  /**
+    @brief     Get pixy camera exposure compensation.
+    @param[out] gain  Camera gain.
+    @param[out] comp  Camera exposure compensation.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_get_exposure_compensation(uint8_t * gain, uint16_t * comp);
+
+  /**
+    @brief     Set pixy camera brightness.
+    @param[in] brightness  Brightness value.
+    @return     0         Success
+    @return     Negative  Error
+  */
+  int pixy_cam_set_brightness(uint8_t brightness);
+
+  /**
+    @brief     Get pixy camera brightness.
+    @return     Non-negative Brightness value.
+    @return     Negative     Error
+  */
+  int pixy_cam_get_brightness();
+
+  /**
+    @brief     Get pixy servo axis position.
+    @param     channel  Channel value. Range: [0, 1]
+    @return     Position of channel. Range: [0, 999]
+    @return     Negative  Error
+  */
+  int pixy_rcs_get_position(uint8_t channel);
+
+  /**
+    @brief     Set pixy servo axis position.
+    @param     channel  Channel value. Range: [0, 1]
+    @param     position Position value of the channel. Range: [0, 999]
+    @return      0         Success
+    @return      Negative  Error
+  */
+  int pixy_rcs_set_position(uint8_t channel, uint16_t position);
+
+  /**
+    @brief     Set pixy servo pulse width modulation (PWM) frequency.
+    @param     frequency Range: [20, 300] Hz Default: 50 Hz
+  */
+  int pixy_rcs_set_frequency(uint16_t frequency);
+
+  /**
+    @brief    Get pixy firmware version.
+    @param[out]  major  Major version component
+    @param[out]  minor  Minor version component
+    @param[out]  build  Build identifier
+    @return      0         Success
+    @return      Negative  Error
+  */
+  int pixy_get_firmware_version(uint16_t * major, uint16_t * minor, uint16_t * build);
+
+  /*@}*/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

common/pixy/pixydefs.h

@@ -0,0 +1,92 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef __PIXYDEFS_H__
+#define __PIXYDEFS_H__
+
+//#include "libusb.h"
+
+//#define PIXY_VID       0xB1AC
+//#define PIXY_DID       0xF000
+//#define PIXY_DFU_VID   0x1FC9
+//#define PIXY_DFU_DID   0x000C
+
+//#define PIXY_ERROR_USB_IO                    LIBUSB_ERROR_IO
+//#define PIXY_ERROR_USB_NOT_FOUND             LIBUSB_ERROR_NOT_FOUND
+//#define PIXY_ERROR_USB_BUSY                  LIBUSB_ERROR_BUSY
+//#define PIXY_ERROR_USB_NO_DEVICE             LIBUSB_ERROR_NO_DEVICE
+#define PIXY_ERROR_INVALID_PARAMETER        -150
+#define PIXY_ERROR_CHIRP                    -151
+#define PIXY_ERROR_INVALID_COMMAND          -152
+
+#define CRP_ARRAY                       0x80 // bit
+#define CRP_FLT                         0x10 // bit
+#define CRP_NO_COPY                     (0x10 | 0x20)
+#define CRP_NULLTERM_ARRAY              (0x20 | CRP_ARRAY) // bits
+#define CRP_INT8                        0x01
+#define CRP_UINT8                       0x01
+#define CRP_INT16                       0x02
+#define CRP_UINT16                      0x02
+#define CRP_INT32                       0x04
+#define CRP_UINT32                      0x04
+#define CRP_FLT32                       (CRP_FLT | 0x04)
+#define CRP_FLT64                       (CRP_FLT | 0x08)
+#define CRP_STRING                      (CRP_NULLTERM_ARRAY | CRP_INT8)
+#define CRP_TYPE_HINT                   0x64 // type hint identifier
+#define CRP_INTS8                       (CRP_INT8 | CRP_ARRAY)
+#define CRP_INTS16                      (CRP_INT16 | CRP_ARRAY)
+#define CRP_INTS32                      (CRP_INT32 | CRP_ARRAY)
+#define CRP_UINTS8                      CRP_INTS8
+#define CRP_UINTS8_NO_COPY              (CRP_INTS8 | CRP_NO_COPY)
+#define CRP_UINTS16_NO_COPY             (CRP_INTS16 | CRP_NO_COPY)
+#define CRP_UINTS32_NO_COPY             (CRP_INTS32 | CRP_NO_COPY)
+#define CRP_UINTS16                     CRP_INTS16
+#define CRP_UINTS32                     CRP_INTS32
+#define CRP_FLTS32                      (CRP_FLT32 | CRP_ARRAY)
+#define CRP_FLTS64                      (CRP_FLT64 | CRP_ARRAY)
+
+// regular call args
+#define INT8(v)                         CRP_INT8, v
+#define UINT8(v)                        CRP_INT8, v
+#define INT16(v)                        CRP_INT16, v
+#define UINT16(v)                       CRP_INT16, v
+#define INT32(v)                        CRP_INT32, v
+#define UINT32(v)                       CRP_INT32, v
+#define FLT32(v)                        CRP_FLT32, v
+#define FLT64(v)                        CRP_FLT64, v
+#define STRING(s)                       CRP_STRING, s
+#define INTS8(len, a)                   CRP_INTS8, len, a
+#define UINTS8(len, a)                  CRP_INTS8, len, a
+#define UINTS8_NO_COPY(len)             CRP_UINTS8_NO_COPY, len
+#define UINTS16_NO_COPY(len)            CRP_UINTS16_NO_COPY, len
+#define UINTS32_NO_COPY(len)            CRP_UINTS32_NO_COPY, len
+#define INTS16(len, a)                  CRP_INTS16, len, a
+#define UINTS16(len, a)                 CRP_INTS16, len, a
+#define INTS32(len, a)                  CRP_INTS32, len, a
+#define UINTS32(len, a)                 CRP_INTS32, len, a
+#define FLTS32(len, a)                  CRP_FLTS32, len, a
+#define FLTS64(len, a)                  CRP_FLTS64, len, a
+
+#ifndef END
+#ifdef __x86_64__
+#define END (int64_t)0
+#else
+#define END 0
+#endif
+#endif
+#define END_OUT_ARGS END
+#define END_IN_ARGS END
+
+#endif

common/system/system.c

@@ -0,0 +1,19 @@
+#include "system.h"
+#include "ll_system.h"
+
+
+bool system_init() {
+	return ll_system_init();
+}
+
+void system_delay(uint32_t msec) {
+	ll_system_delay(msec);
+}
+
+void system_process() {
+	ll_system_process();
+}
+
+void system_toggle_led() {
+	ll_system_toggle_led();
+}

common/system/system.h

@@ -0,0 +1,38 @@
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @defgroup system System
+ * The System Module provides access to delay functions, leds and provides a system init function
+ */
+/*@{*/
+
+
+/**
+ * Initializes the system. Call this method at the start of your app_init() function and before using any system_* functions
+ * @return true on success
+ */
+bool system_init();
+
+/**
+ * Sleeps for a certain amount of time
+ * @param msec The number of milliseconds to sleep
+ */
+void system_delay(uint32_t msec);
+
+/**
+ * Executes pending system events (like handling usb, timers etc). Call this somewhere in app_process().
+ */
+void system_process();
+
+/**
+ * Toggles a Status Led. Use this function for debugging or to show activity
+ */
+void system_toggle_led();
+
+/*@}*/
+
+#endif /* SYSTEM_H */

common/tft/tft.c

@@ -0,0 +1,143 @@
+#include "tft.h"
+#include "ll_tft.h"
+#include <string.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include "filesystem.h"
+
+/* The idea is as follows:
+ * Most of the tft_* functions can be forwarded to the lowlevel implementation.
+ * The exceptions are commented below.
+ * Make sure to have a look at the doxygen comments for the lowlevel functions and for the tft_* functions
+ */
+
+/* Possible improvements:
+ * For formatted printing implement putchar, instead of writing into a buffer and drawing that buffer afterwards
+ */
+
+bool tft_init() {
+	return ll_tft_init();
+
+}
+
+void tft_clear(uint16_t color) {
+	ll_tft_clear(color);
+}
+
+void tft_draw_line(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color) {
+	ll_tft_draw_line(x1,y1,x2,y2,color);
+}
+
+
+void tft_draw_pixel(uint16_t x,uint16_t y,uint16_t color) {
+	ll_tft_draw_pixel(x,y,color);
+}
+
+void tft_draw_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color) {
+	//could be implemented with 4 lines instead of introducing a ll func
+	ll_tft_draw_rectangle(x1,y1,x2,y2,color);
+}
+
+void tft_fill_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color) {
+	ll_tft_fill_rectangle(x1,y1,x2,y2,color);
+}
+
+void tft_draw_bitmap_unscaled(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint16_t* dat) {
+	ll_tft_draw_bitmap_unscaled(x,y,width,height,dat);
+}
+
+void tft_draw_circle(uint16_t x, uint16_t y, uint16_t r, uint16_t color) {
+    ll_tft_draw_circle(x, y, r, color);
+}
+
+uint8_t tft_num_fonts() {
+	return ll_tft_num_fonts();
+}
+
+uint8_t tft_font_height(uint8_t fontnum) {
+	return ll_tft_font_height(fontnum);
+}
+
+uint8_t tft_font_width(uint8_t fontnum) {
+	return ll_tft_font_width(fontnum); 
+}
+
+//Print line can be done with multiple calls to draw_char
+void tft_print_line(uint16_t x, uint16_t y, uint16_t color, uint16_t bgcolor, uint8_t font, const char* text) {
+	if(font>=ll_tft_num_fonts()) return; //invalid font index
+	for(int i=0; i<strlen(text); i++) { //for each char in the line
+		ll_tft_draw_char(x,y,color,bgcolor, font, text[i]); //draw the char
+		x+=ll_tft_font_width(font);	//and increase the x position
+	}
+}
+
+//Printing a formatted line can be done by printing the line in a buffer using "sprintf" and then calling print_line
+void tft_print_formatted(uint16_t x, uint16_t y, uint16_t color, uint16_t bgcolor, uint8_t font, const char* format, ...) {
+	static char buffer[128]; //buffer to save the formatted text into
+
+	//Since we have variable arguments, we need to forward them. We have to use vsprintf instead of sprintf for that.
+	va_list args;
+	va_start (args, format); //start the varg-list
+	vsprintf(buffer,format,args); //let vsprintf render the formatted string
+	tft_print_line(x,y,color,bgcolor,font,buffer); //print the string as normal text
+	va_end(args); //end the varg-list
+}
+
+bool tft_draw_bitmap_file_unscaled(uint16_t x, uint16_t y, const char* filename) {
+	//This method reads a .bmp file from the filesystem and tries to draw it.
+	//Note: The bmp implementation is not complete, it has some limitations and it makes assumptions. See doxygen comment for this method.
+	//Source Copied and adapted from: http://stackoverflow.com/a/17040962/2606757
+
+	FILE_HANDLE* file = filesystem_file_open(filename); //try to open the file
+	if(file==NULL) { //file opening failed
+		return false;
+	}
+
+	unsigned char info[54];
+	if(filesystem_file_read(file,info,54)!=F_OK) { //try to read the 54 byte header
+		filesystem_file_close(file);
+		return false; //reading the header failed
+	}
+
+	// extract image height and width from header
+	uint32_t width = *(uint32_t*)&info[18]; //width in pixel
+	uint32_t height = *(uint32_t*)&info[22]; //height in pixel
+	uint16_t depth = *(uint16_t*)&info[28]; //bit's per pixel (color depth)
+	depth/=8; //we want the number of bytes per pixel
+
+	filesystem_file_seek(file,*(uint32_t*)&info[10]); //seek to the place where img data begins
+
+	uint32_t row_padded = (width*depth + 3) & (~3); //row size must be aligned to 4 bytes
+
+	unsigned char data [row_padded]; //allocate space for one row (incl. padding)
+
+	for(int i = 0; i < height; i++) //for each row
+	{
+		filesystem_file_read(file,data,row_padded); //read row into buffer
+		for(int j = 0; j < width*depth; j += depth) //for each pixel
+		{
+			unsigned char a,r,g,b;
+			if(depth==4) { //a,r,g,b 8bit each
+				a = data[j];
+				r = data[j+1];
+				g = data[j+2];
+				b = data[j+3];
+			} else if (depth==3) { // b,g,r, 8bit each
+				a = 255;
+				r = data[j+2];
+				g = data[j+1];
+				b = data[j];
+			}
+
+			if(a!=0) {
+				//bmp's are stored "bottom-up", so we start drawing at the bottom
+				tft_draw_pixel(x+j/depth,y+height-1-i,RGB(r,g,b));
+			}
+		}
+	}
+
+	filesystem_file_close(file);
+
+	return true;
+
+}

common/tft/tft.h

@@ -0,0 +1,172 @@
+#ifndef TFT_H
+#define TFT_H
+
+#include<stdbool.h>
+#include<stdint.h>
+
+/**
+ * @defgroup tft TFT
+ * The TFT Modul provides access to the display
+ */
+
+
+/**
+ * @addtogroup tft
+ */
+/*@{*/
+
+
+/**
+ * Creates a 16bit color from 8bit * 3 colors (r,g,b)
+ * @return
+ */
+#define RGB(r,g,b) ((((r) & 0xF8) << 8) | (((g) & 0xFC) << 3) | (((b) & 0xF8) >> 3))
+
+#define RED RGB(255,0,0)
+#define GREEN RGB(0,255,0)
+#define BLUE RGB(0,0,255)
+#define WHITE 0xF7BE
+#define BLACK RGB(0,0,0)
+
+/**
+ * Creates a 16bit color from a 24bit hex rgb color code
+ * @return
+ */
+#define HEX(h) (RGB(((h)>>16),((h)>>8),(h)))
+
+/**
+ * Transparent color
+ * @return
+ */
+#define TRANSPARENT ((uint16_t)0x80C2)
+
+/**
+ * Initializes the display.
+ * Call this method before using any tft_* functions
+ * @return true on success
+ */
+bool tft_init();
+
+/**
+ * Clears the entire display with the given color. Overpaints everything which was there before.
+ * @param color The 16-bit color to clear the display with.
+ */
+void tft_clear(uint16_t color);
+
+/**
+ * Draws a line onto the display. The pixels specified by start/end point are inclusive!
+ * @param x1 The x-Coordinate of the start-point
+ * @param y1 The y-Coordinate of the start-point
+ * @param x2 The x-Coordinate of the end-point
+ * @param y2 The y-Coordinate of the end-point
+ * @param color The 16-bit color to draw the line with
+ */
+void tft_draw_line(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color);
+
+/**
+ * Draws a pixel onto the display.
+ * @param x The x-Coordinate of the pixel
+ * @param y The y-Coordinate of the pixel
+ * @param color The 16-bit color to draw the pixel with
+ */
+void tft_draw_pixel(uint16_t x,uint16_t y,uint16_t color);
+
+/**
+ * Draws the outline of a rectangle onto the display.
+ * The outline is one pixel wide and goes through the specified start and endpoint.
+ * @param x1 The x-Coordinate of the start-point
+ * @param y1 The y-Coordinate of the start-point
+ * @param x2 The x-Coordinate of the end-point
+ * @param y2 The y-Coordinate of the end-point
+ * @param color The 16-bit color to draw the pixel with
+ */
+void tft_draw_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color);
+
+/**
+ * Draws a filled rectangle onto the display. The start,end points are inclusive
+ * @param x1 The x-Coordinate of the start-point
+ * @param y1 The y-Coordinate of the start-point
+ * @param x2 The x-Coordinate of the end-point
+ * @param y2 The y-Coordinate of the end-point
+ * @param color The 16-bit color to draw the pixel with
+ */
+void tft_fill_rectangle(uint16_t x1,uint16_t y1,uint16_t x2,uint16_t y2, uint16_t color);
+
+/**
+ * Draws a bitmap onto the display without scaling/cropping.
+ * The bitmap must be provided as an array of 16-bit colors
+ * @param x The x-coordinate of the top-left corner to draw the bitmap at
+ * @param y The y-coordinate of the top-left corner to draw the bitmap at
+ * @param width The width of the bitmap in pixels
+ * @param height The height of the bitmap in pixels
+ * @param dat A pointer to a uint16_t array containing the colors for each pixel. Starting in the topleft and going from left to right, line by line.
+ */
+void tft_draw_bitmap_unscaled(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint16_t* dat);
+
+
+/**
+ * Draws a bitmap from the filesystem onto the display without scaling/cropping
+ * The bitmap must be saved in the windows bitmap format (.bmp) without compression and with 24 (b,g,r) or 32 (a,r,g,b) bits per pixel
+ * @param x The x-coordinate of the top-left corner to draw the bitmap at
+ * @param y The y-coordinate of the top-left corner to draw the bitmap at
+ * @param filename The absolute path to the .bmp file
+ * @return true on success
+ */
+bool tft_draw_bitmap_file_unscaled(uint16_t x, uint16_t y, const char* filename);
+
+
+/**
+ * Draws the outline of a circle onto the display
+ * @param x The x-Coordinate of the center point
+ * @param y The y-Coordinate of the center point
+ * @param r The Radius in Pixels
+ * @param color The 16-Bit color to draw the circle with
+ */
+void tft_draw_circle(uint16_t x, uint16_t y, uint16_t r, uint16_t color);
+
+/**
+ * Queries the number of available fonts
+ * @return
+ */
+uint8_t tft_num_fonts();
+
+/**
+ * Get the height of a font
+ * @param fontnum The number of the font, from 0 .. (num_fonts -1)
+ * @return The height in pixel
+ */
+uint8_t tft_font_height(uint8_t fontnum);
+
+/**
+ * Get the width of a font
+ * @param fontnum The number of the font, from 0 .. (num_fonts -1)
+ * @return The width in pixel
+ */
+uint8_t tft_font_width(uint8_t fontnum);
+
+/**
+ * Prints a unformatted/preformatted string onto the display
+ * @param x The x-Coordinate of the Top-Left corner where the text should be drawn
+ * @param y The y-Coordinate of the Top-Left corner where the text should be drawn
+ * @param color The 16-bit foreground color of the text
+ * @param bgcolor The 16-bit background color of the text. You may pass TRANSPARENT as Color
+ * @param font The Fontnum to use for drawing
+ * @param text The text to draw
+ */
+void tft_print_line(uint16_t x, uint16_t y, uint16_t color, uint16_t bgcolor, uint8_t font, const char* text);
+
+/**
+ * Prints a formatted text (like printf) onto the display
+ * @param x The x-Coordinate of the Top-Left corner where the text should be drawn
+ * @param y The y-Coordinate of the Top-Left corner where the text should be drawn
+ * @param color The 16-bit foreground color of the text
+ * @param bgcolor The 16-bit background color of the text. You may pass TRANSPARENT as Color
+ * @param font The Fontnum to use for drawing
+ * @param format The format string (like printf)
+ * @param ... The arguments to format (like printf)
+ */
+void tft_print_formatted(uint16_t x, uint16_t y, uint16_t color, uint16_t bgcolor, uint8_t font, const char* format, ...);
+
+/*@}*/
+
+#endif /* TFT_H */

common/touch/screen_calibrate.c

@@ -0,0 +1,95 @@
+#include "screen_calibrate.h"
+#include "tft.h"
+#include "touch.h"
+
+
+extern volatile bool calibration; //from touch.c
+
+
+static void enter(void* screen) {
+	tft_clear(BLACK);
+}
+
+static void leave(void* screen) {
+
+}
+
+static void update(void* screen) {
+	int x1,y1,x2,y2,dx,dy;
+
+
+	tft_print_line(50,50,WHITE,BLACK,1,"Calibration:");
+	tft_print_line(50,120,WHITE,BLACK,0,"Hit the markers exactly!");
+	//-----------------First Point--------------------
+	tft_draw_line(CCENTER,CBEGIN,CCENTER,CEND,WHITE);	//Draw Cross
+	tft_draw_line(CBEGIN,CCENTER,CEND,CCENTER,WHITE);	//Draw Cross
+	calibration=1; //TouchX + TouchY Values will not be converted to Pixels
+	while(calibration);	//Wait on PenUp
+	POINT_STRUCT p1 = touch_get_last_point();
+	x1=p1.x;
+	y1=p1.y;
+	tft_fill_rectangle(CBEGIN,CBEGIN,CEND,CEND,BLACK); //Clear Cross
+
+	//-----------------Second Point-------------------
+	tft_draw_line(DWIDTH-CCENTER,DHEIGHT-CBEGIN,DWIDTH-CCENTER,DHEIGHT-CEND,WHITE);
+	tft_draw_line(DWIDTH-CBEGIN,DHEIGHT-CCENTER,DWIDTH-CEND,DHEIGHT-CCENTER,WHITE);
+	calibration=1;
+	while(calibration);
+	POINT_STRUCT p2 = touch_get_last_point();
+	x2=p2.x;
+	y2=p2.y;
+	tft_fill_rectangle(DWIDTH-CBEGIN,DHEIGHT-CBEGIN,DWIDTH-CEND,DHEIGHT-CEND,BLACK);
+
+	//-----------------Third Point--------------------
+	tft_draw_line(CCENTER,DHEIGHT-CBEGIN,CCENTER,DHEIGHT-CEND,WHITE);
+	tft_draw_line(CBEGIN,DHEIGHT-CCENTER,CEND,DHEIGHT-CCENTER,WHITE);
+		calibration=1;
+	while(calibration);
+	POINT_STRUCT p3 = touch_get_last_point();
+	x1+=p3.x; //Add(!) values. We'll build the average later
+	y2+=p3.y;
+	tft_fill_rectangle(CBEGIN,DHEIGHT-CBEGIN,CEND,DHEIGHT-CEND,BLACK);
+
+	//------------------4. Point---------------------
+	tft_draw_line(DWIDTH-CCENTER,CBEGIN,DWIDTH-CCENTER,CEND,WHITE);
+	tft_draw_line(DWIDTH-CBEGIN,CCENTER,DWIDTH-CEND,CCENTER,WHITE);
+	calibration=1;
+	while(calibration);
+	POINT_STRUCT p4 = touch_get_last_point();
+	x2+=p4.x;
+	y1+=p4.y;
+	tft_fill_rectangle(DWIDTH-CBEGIN,CBEGIN,DWIDTH-CEND,CEND,BLACK);
+	//-------------------Calculation---------------------
+	x1++; //Add 1 and divide by 2 later = +0.5 (for correct rounding)
+	y1++;
+	x2++;
+	y2++;
+	x1>>=1; //Divide by 2
+	y1>>=1;
+	x2>>=1;
+	y2>>=1;
+	dx = (x2-x1);	//Build the Difference
+	dy = (y2-y1);
+
+	touch_set_calibration_values(x1,dx,y1,dy);
+	tft_print_line(50,120,WHITE,BLACK,0,"Calibration Done. Press anywhere");
+
+	calibration=1;
+	while(calibration);
+	gui_screen_back();
+
+}
+
+
+static SCREEN_STRUCT screen = {
+	enter,
+	leave,
+	update
+};
+
+
+SCREEN_STRUCT* get_screen_calibrate() {
+	return &screen;
+}
+
+

common/touch/screen_calibrate.h

@@ -0,0 +1,33 @@
+#include "screen.h"
+
+/**
+ * @addtogroup touch
+ */
+/*@{*/
+
+
+/**
+ * @defgroup calibrate Calibrate (Screen)
+ * The calibrate screen for the touch module
+ */
+/*@{*/
+
+
+/**
+ * Returns a pointer to the calibrate screen
+ * \sa gui_screen_navigate
+ * @return
+ */
+SCREEN_STRUCT* get_screen_calibrate();
+
+/*@}*/
+/*@}*/
+
+
+//TODO: Move this define to a common accessible, but private header file (they are used by screen_calibrate.c and touch.c)
+#define CCENTER 20  //Pixel Distance from Sides for Calibration Cross
+#define CLENGTH 10	//Length of the Calibration Cross Lines
+#define CBEGIN (CCENTER-CLENGTH/2)
+#define CEND (CCENTER + CLENGTH/2)
+#define DWIDTH 320 //TODO: move define to tft module or make a function out of it
+#define DHEIGHT 240 //TODO: move define to tft module or make a function out of it

common/touch/touch.c

@@ -0,0 +1,201 @@
+#include "touch.h"
+#include "ll_touch.h"
+#include "screen_calibrate.h"
+#include <stdio.h>
+
+/* The idea is as follows:
+ * The user can add "touch-areas" which basically represent a rectangles on the screen.
+ * Once the user touches such a rectangle with the pen, we forward events to his provided callback.
+ * Touch events are provided to us from the low level implementation via touch_add_raw_event().
+ * We then need to check which touch areas are effected by that event
+ */
+
+/* Possible improvements:
+ * Exchange pointer-list "areas" with a linked list. This would ensure that we can always accept new regions
+ */
+
+#define NUM_AREAS 50    //Number of Touch Areas we can manage
+TOUCH_AREA_STRUCT* areas[NUM_AREAS] = {NULL}; //list with pointers to all managed touch area's
+
+volatile POINT_STRUCT pos; //the last touch point
+volatile TOUCH_STATE oldState=TOUCH_UP; //the last touch state
+volatile bool calibration = false; //whether or not we're currently calibrating
+
+bool use_calibration=false; //Whether or not the current platform needs calibration and recalc of the values
+
+//Calibration parameters (dummy values).
+int cal_xs=10;
+int cal_dx=100;
+int cal_ys=10;
+int cal_dy=100;
+
+
+void touch_set_calibration_values(int xs, int dx, int ys, int dy) {
+	cal_xs = xs;
+	cal_ys = ys;
+	cal_dx = dx;
+	cal_dy = dy;
+}
+
+
+
+bool touch_init() {
+	return ll_touch_init();
+}
+
+void touch_set_value_convert_mode(bool uc) {
+	use_calibration=uc;
+}
+
+
+bool touch_add_raw_event(uint16_t touchX, uint16_t touchY, TOUCH_STATE state) {
+	//Update current and old position/state
+	bool penDown = (state==TOUCH_DOWN);
+	bool oldPenDown = (oldState==TOUCH_DOWN);
+	oldState=state;
+
+	if(calibration)	//If in Calibration mode
+	{
+		if(penDown)
+		{
+			pos.x=touchX;
+			pos.y=touchY;
+		}
+		else
+		{
+			if(oldPenDown) //Run only if we got at least one pen down
+				calibration=0;	 //Calibration finish (Touch X and Y are the values from the last measure, where the pen was down)
+		}
+		return true;
+	}
+
+	//If we reach this point we're not in calibration mode and we need to process the event and call the registred handlers..
+
+	if(use_calibration) { //the underlying touch hardware uses calibration
+		//Calculate the real touch position out of the passed ones, and the calibration values
+		pos.x=touchX=(((long)(DWIDTH-2*CCENTER)*2*(long)((long)touchX-cal_xs)/cal_dx+1)>>1)+CCENTER;
+		pos.y=touchY=(((long)(DHEIGHT-2*CCENTER)*2*(long)((long)touchY-cal_ys)/cal_dy+1)>>1)+CCENTER;
+	} else { //no conversion needed for the underlying hardware
+		pos.x=touchX;
+		pos.y=touchY;
+	}
+
+ 	if(penDown) //pen is down now
+    {
+ 		//tft_draw_pixel(touchX,touchY,WHITE);
+		if(!oldPenDown) //pen wasn't down before (positive edge) => First Touch
+		{
+			for(int z=0; z < NUM_AREAS; z++) // For every touch area
+			{
+				//Check if pos is inside area
+				if(areas[z]!=NULL  && touchX >= areas[z]->x1 && touchX <= areas[z]->x2 && touchY >= areas[z]->y1 && touchY <= areas[z]->y2 )
+				{
+					areas[z]->flags=1; //Save PenInside=1
+					if(areas[z]->hookedActions & PEN_DOWN) //The user wants to receive pen down events
+						areas[z]->callback(areas[z],PEN_DOWN); //Send event to user callback
+				}
+			}
+		}
+		else //Pen was down before => Second, Third event in row
+		{
+			for(int z=0; z < NUM_AREAS; z++) // For every touch area
+			{
+				if(areas[z]!=NULL )
+				{
+					//Check if pos is inside area
+					if(touchX >= areas[z]->x1 && touchX <= areas[z]->x2 && touchY >= areas[z]->y1 && touchY <= areas[z]->y2)
+					{
+						if(areas[z]->flags==0) //Pen was not inside before (PenInside==0)
+						{
+							areas[z]->flags=1; //Pen is inside now (PenInside=1)
+							if(areas[z]->hookedActions & PEN_ENTER) //The user wants to receive pen enter events
+								areas[z]->callback(areas[z],PEN_ENTER);
+						}
+					}
+					else if(areas[z]->flags) //Pos not inside area, but it was before (PenInside==1)
+					{
+						areas[z]->flags=0; //Pen is no longer inside (PenInside=0)
+						if(areas[z]->hookedActions & PEN_LEAVE) //The user wants to receive pen leave events
+							areas[z]->callback(areas[z],PEN_LEAVE);
+					}
+				}
+			}
+		}
+		for(int z=0; z < NUM_AREAS; z++) // For every touch area
+		{
+			if(areas[z]!=NULL && (areas[z]->hookedActions&PEN_MOVE)) //User want's to receive pen move events
+			{
+				//Check if pos is inside area
+				if(touchX >= areas[z]->x1 && touchX <= areas[z]->x2 && touchY >= areas[z]->y1 && touchY <= areas[z]->y2)
+				{
+					areas[z]->callback(areas[z],PEN_MOVE);
+				}
+			}
+		}
+	}
+	else //pen is not down now
+	{
+		if(oldPenDown) //but it was down before (negative edge)
+		{
+			for(int z=0; z < NUM_AREAS; z++) // For every touch area
+			{
+				//Check if pos is inside area
+				if(areas[z]!=NULL && touchX >= areas[z]->x1 && touchX <= areas[z]->x2 && touchY >= areas[z]->y1 && touchY <= areas[z]->y2 )
+				{
+					areas[z]->flags=0; //The pen is no longer inside (PenInside = 0);
+					if(areas[z]->hookedActions & PEN_UP) //user want's to receive pen up events
+						areas[z]->callback(areas[z],PEN_UP);
+				}
+			}
+		}
+	}
+	return true;
+}
+
+bool touch_have_empty(unsigned char num)
+{
+	//go through pointer array and check for free spaces
+	for(unsigned char i=0; i<NUM_AREAS; i++)
+	{
+		if(areas[i]==NULL) num--; //a free space was found, we need one less
+		if(num==0) return true; //enough free spaces found
+	}
+	return false; //not enough free spaces found
+}
+
+bool touch_register_area(TOUCH_AREA_STRUCT* area)
+{
+	//go through pointer array and check for free space
+	for(unsigned char i=0; i<NUM_AREAS; i++)
+	{
+		if(areas[i]==NULL) //free space found
+		{
+			area->flags=0; //we start with empty flags (PenInside=0)
+			areas[i]=area; //save pointer into list
+			return true;
+		}
+	}
+	return false; //no free space found
+}
+
+void touch_unregister_area(TOUCH_AREA_STRUCT* area)
+{
+	if(area==NULL) return;
+
+	//go through pointer array and find the area to remove
+	for(unsigned char i=0; i<NUM_AREAS; i++)
+	{
+		if(areas[i]==area) //area found in pointer array at pos i
+		{
+			areas[i]=NULL; //set pointer in list to NULL again
+			break;
+		}
+	}
+}
+
+
+POINT_STRUCT touch_get_last_point() {
+	return pos;
+}
+
+

common/touch/touch.h

@@ -0,0 +1,131 @@
+#ifndef TOUCH_H
+#define TOUCH_H
+
+#include<stdbool.h>
+#include<stdint.h>
+
+/**
+ * @defgroup touch Touch
+ * The Touch module provides access to the touch controller, and executes a callback if a certain region is touched
+ */
+
+
+/**
+ * @addtogroup touch
+ */
+/*@{*/
+
+/**
+Enum to describe the current Touch State. \sa touch_add_raw_event
+ */
+typedef enum {
+	TOUCH_UP, //!< The display is currently not touched
+	TOUCH_DOWN //!< The display is currently touched at some point
+} TOUCH_STATE ;
+
+/**
+ * Enum to describe the hooked actions for which you want to receive events for.
+ * You can OR-combine them. \sa touch_register_area
+ */
+typedef enum {
+	NONE=0x00,      //!< Do not receive any events
+	PEN_DOWN=0x01,  //!< Receive an event when the pen goes down inside the region
+	PEN_UP=0x02,    //!< Receive an event when the pen goes up inside the region
+	PEN_ENTER=0x04, //!< Receive an event when the pen enters the region (pen was down before)
+	PEN_LEAVE=0x08, //!< Receive an event when the pen leaves the region (pen was inside region before)
+	PEN_MOVE=0x10   //!< Receive an event when the pen moves inside the region (pen is down)
+} TOUCH_ACTION;
+
+/**
+ * Prototype for Event Listeners (called for every occurring, hooked action)
+ * \note You should NOT execute long running things in this callback nor should you update the gui. But you can call gui_screen_navigate() for instance.
+ * @param touchArea The pointer to the TOUCH_AREA_STRUCT in which the event occurred
+ * @param triggeredAction The Action which occurred
+ */
+typedef void (*TOUCH_CALLBACK)(void* touchArea, TOUCH_ACTION triggeredAction);
+
+/**
+ * Structure to configure a Touch Area
+ */
+typedef struct {
+        TOUCH_ACTION hookedActions; //!< Actions to listen to
+        uint16_t x1; //!< Top Left X-Coordinate of Area
+        uint16_t y1; //!< Top Left Y-Coordinate of Area
+        uint16_t x2; //!< Bottom Right X-Coordinate of Area
+        uint16_t y2; //!< Bottom Right Y-Coordinate of Area
+        TOUCH_CALLBACK callback; //!< Callback which is executed when an event occurred in this Area.
+        uint8_t flags; //!< For internal use, don't change, don't initialize
+} TOUCH_AREA_STRUCT;
+
+
+/**
+ * Struct which represents a 2D point on the display
+ */
+typedef struct {
+	uint16_t x; //!< The X-Coordinate of the point
+	uint16_t y; //!< The Y-Coordinate of the point
+} POINT_STRUCT;
+
+/**
+ * Initializes the Touch Controller.
+ * Call this method before using any touch_* functions
+ * @return true on success
+ */
+bool touch_init();
+
+/**
+ * Processes a native touch event.
+ * Call this function when the pen goes down (\ref TOUCH_DOWN), when it moves (\ref TOUCH_DOWN) and also when it goes up again (\ref TOUCH_UP)!
+ * It's safe to call this function from an (SPI)-Interrupt.
+ * @param x The x-Coordinate of the touch event
+ * @param y The y-Coordinate of the touch event
+ * @param state Whether the pen is up or down
+ * @return True on success
+ */
+bool touch_add_raw_event(uint16_t x, uint16_t y,TOUCH_STATE state);
+
+/**
+ * Checks whether or not we have memory to manage and track additional \p num TOUCH_AREA_STRUCT%s
+ * @param num The number of touch areas you would like to allocate
+ * @return True if there's enough memory to allocate num TOUCH_AREAs
+ */
+bool touch_have_empty(unsigned char num);
+
+/**
+ * Registers a new touch Area. You will receive events for this area from now on.
+ * @param area A pointer to the configured TOUCH_AREA_STRUCT
+ * @return True if everything was successful and the corresponding Touch Area will be monitored from now on
+ */
+bool touch_register_area(TOUCH_AREA_STRUCT* area);
+
+/**
+ * Unregisters a touch area. You will no longer receive events for this area
+ * @param area A pointer to the TOUCH_AREA_STRUCT instance
+ */
+void touch_unregister_area(TOUCH_AREA_STRUCT* area);
+
+/**
+ * Gets the last touched point
+ * @return The Coordinates of the last touched points
+ */
+POINT_STRUCT touch_get_last_point();
+
+/**
+ * Set's the new calibration values
+ * @param xs x offset (to calibration point 1)
+ * @param dx x difference (between calibration point 1 and 2)
+ * @param ys y offset (to calibration point 1)
+ * @param dy y difference (between calibration point 1 and 2)
+ */
+void touch_set_calibration_values(int xs, int dx, int ys, int dy);
+
+
+/**
+ * Set's the new value convert mode. Per default use_calibration is false.
+ * @param use_calibration whether or not the current platform needs display calibration
+ */
+void touch_set_value_convert_mode(bool use_calibration);
+
+/*@}*/
+
+#endif /* TOUCH_H */

discovery/.gitignore

@@ -3,3 +3,8 @@ obj/
 
 libs/*/obj
 libs/*/*.a
+libs/*/*.o
+
+*~
+*.swp
+*.swo

discovery/Makefile

@@ -1,13 +1,14 @@
 #2015 by tmoe, id10101 (and the internet :) )
 
 #Name of the binary/project
-TARGET=hello
+TARGET=discoverpixy
 
 #Tools
 CROSS_COMPILE=arm-none-eabi-
-CC=$(CROSS_COMPILE)gcc
+CC=$(CROSS_COMPILE)gcc -fdiagnostics-color=auto
 OBJCOPY=$(CROSS_COMPILE)objcopy
 GDB=$(CROSS_COMPILE)gdb
+SIZE=$(CROSS_COMPILE)size
 
 MKDIR=mkdir -p
 RM=rm -f
@@ -21,32 +22,56 @@ SRC_DIR=./src
 OBJ_DIR=./obj
 BUILD_DIR=./build
 LIB_DIR=./libs
+COMMON_DIR=../common
+
 
 #Architecture flags
 FP_FLAGS?=-mfpu=fpv4-sp-d16 -mfloat-abi=softfp
 ARCH_FLAGS=-mthumb -mcpu=cortex-m4 $(FP_FLAGS)
 
 #Compiler, Linker Options
-CPPFLAGS=-I$(LIB_DIR)/StmCoreNPheriph/inc
+#LIB_FOLDERS=$(shell  find $(LIB_DIR) -maxdepth 1 -type d ! -path $(LIB_DIR))
-CFLAGS=$(ARCH_FLAGS) -O0 -g
+INCLUDES=$(LIB_DIR)/StmCoreNPheriph/inc 
+INCLUDES+=$(LIB_DIR)/StmUsbHost/STM32_USB_Device_Specific 
+INCLUDES+=$(LIB_DIR)/StmUsbHost/STM32_USB_OTG_Driver/inc
+INCLUDES+=$(LIB_DIR)/StmUsbHost/STM32_USB_HOST_Library/Core/inc 
+INCLUDES+=$(shell  find $(COMMON_DIR) -maxdepth 1 -type d ! -path $(COMMON_DIR))
 
-LDFLAGS=--specs=nosys.specs
+INCLUDES:=$(addprefix -I,$(INCLUDES))
+
+
+CPPFLAGS=-DUSE_USB_OTG_FS -DUSE_HOST_MODE $(INCLUDES) 
+CFLAGS=$(ARCH_FLAGS) -O0 -g -std=c99 -fdata-sections -ffunction-sections
+
+
+LIBS=pixy usbhost coreperiph stdc++
+LIBSEARCHDIRS=$(LIB_DIR)/StmCoreNPheriph 
+LIBSEARCHDIRS+=$(LIB_DIR)/StmUsbHost 
+LIBSEARCHDIRS+=$(LIB_DIR)/Pixy
+
+
+LDFLAGS=--specs=nano.specs -Wl,--gc-sections
+LDFLAGS+=$(addprefix -L,$(LIBSEARCHDIRS)) 
+LDFLAGS+=$(addprefix -l,$(LIBS))
 
 #Finding Input files
 CFILES=$(shell find $(SRC_DIR) -name '*.c')
 SFILES=$(SRC_DIR)/startup.s
+COMMON_CFILES=$(shell find $(COMMON_DIR) -name '*.c')
+
 
 #Generate corresponding obj names
 SOBJS=$(SFILES:.s=.o)
 COBJS=$(CFILES:.c=.o)
 OBJS=$(patsubst $(SRC_DIR)/%,$(OBJ_DIR)/%,$(SOBJS) $(COBJS))
-
+COMMON_OBJS=$(patsubst $(COMMON_DIR)/%,$(OBJ_DIR)/%,$(COMMON_CFILES:.c=.o))
-#Keep the objects files
-.SECONDARY: $(OBJS)
 
 #Mark targets which are not "file-targets"
 .PHONY: all debug flash start stop backup clean
 
+#keep objs files
+.SECONDARY: $(OBJS) $(COMMON_OBJS)
+
 # List of all binaries to build
 all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).bin
 
@@ -57,14 +82,16 @@ stop:
 	$(STUTIL) stop
 
 #objects to elf
-%.elf : $(OBJS)
+%.elf : $(OBJS) $(COMMON_OBJS)
 	@echo Linking...
 	$(MKDIR) $(BUILD_DIR)
-	$(CC) -o $@ $(CFLAGS) $(CPPFLAGS) $(LDFLAGS) -T./utils/stm32_flash.ld $^
+	$(CC) -o $@ $(CFLAGS) $(CPPFLAGS) -T./utils/stm32_flash.ld -Wl,-Map,$(BUILD_DIR)/$(TARGET).map $^ $(LDFLAGS) 
+	#$(CC) -o $@ $(CFLAGS) $(CPPFLAGS) $(LDFLAGS) -Wl,--verbose -Wl,-Map,$(BUILD_DIR)/$(TARGET).map $^
 
 #elf to binary
 %.bin: %.elf
 	$(OBJCOPY) -O binary $< $@
+	$(SIZE) $<
 
 #Asm files to objects
 $(OBJ_DIR)/%.o: $(SRC_DIR)/%.s
@@ -78,6 +105,12 @@ $(OBJ_DIR)/%.o: $(SRC_DIR)/%.c
 	$(MKDIR) $(OBJ_DIR)
 	$(CC) $(CFLAGS) $(CPPFLAGS) -c -o $@ $<
 
+#common C files to objects
+$(OBJ_DIR)/%.o: $(COMMON_DIR)/%.c
+	@echo Compiling Common file $<...
+	$(MKDIR) $(dir $(patsubst $(COMMON_DIR)/%,$(OBJ_DIR)/%, $<))
+	$(CC) $(CFLAGS) $(CPPFLAGS) -c -o $@ $<
+
 #Clean Obj files and builded stuff
 clean:
 	$(RMDIR) $(BUILD_DIR) $(OBJ_DIR)

discovery/libs/Pixy/.gitignore

@@ -0,0 +1,2 @@
+obj
+*.a

discovery/libs/Pixy/Makefile

@@ -0,0 +1,59 @@
+#2015 by tmoe, id10101 (and the internet :) )
+
+TARGET=libpixy
+
+#Tools
+CROSS_COMPILE=arm-none-eabi-
+CC=$(CROSS_COMPILE)g++
+AR=$(CROSS_COMPILE)ar
+RMDIR = rm -rf
+RM=rm -f
+MKDIR=mkdir -p
+
+#Directories
+SRC_DIR=./src
+INC_DIR=../../../common/pixy
+OBJ_DIR=./obj
+
+#Architecture flags
+FP_FLAGS?=-mfpu=fpv4-sp-d16 -mfloat-abi=softfp
+ARCH_FLAGS=-mthumb -mcpu=cortex-m4 $(FP_FLAGS)
+
+#Compiler, Linker Options
+CPPFLAGS=-I$(INC_DIR) -D__LINUX__=1 -DHOST=1 #-DDEBUG=1
+CFLAGS=$(ARCH_FLAGS) -O0 -g -fdata-sections -ffunction-sections
+#CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 -mthumb-interwork
+#CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
+
+
+#Finding Input files
+CFILES=$(shell find $(SRC_DIR) -name '*.cpp')
+
+#Generate corresponding obj names
+COBJS=$(CFILES:.cpp=.o)
+OBJS=$(patsubst $(SRC_DIR)/%,$(OBJ_DIR)/%,$(COBJS))
+
+#Keep the objects files
+.SECONDARY: $(OBJS)
+
+#Mark targets which are not "file-targets"
+.PHONY: all  clean
+
+# List of all binaries to build
+all: $(TARGET).a
+
+#objects to lib
+%.a : $(OBJS)
+	@echo Linking...
+	$(AR) rcs $@ $^
+
+#C files to objects
+$(OBJ_DIR)/%.o: $(SRC_DIR)/%.cpp
+	@echo Compiling $<...
+	$(MKDIR) $(OBJ_DIR)
+	$(CC) $(CFLAGS) $(CPPFLAGS) -c -o $@ $<
+
+#Clean Obj files and builded stuff
+clean:
+	$(RMDIR)  $(OBJ_DIR)
+	$(RM) $(TARGET).a

discovery/libs/Pixy/hello_pixy.cpp

@@ -0,0 +1,150 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <signal.h>
+#include <string.h>
+#include "pixy.h"
+
+#define BLOCK_BUFFER_SIZE    25
+
+// Pixy Block buffer // 
+struct Block blocks[BLOCK_BUFFER_SIZE];
+
+static bool run_flag = true;
+
+void handle_SIGINT(int unused)
+{
+  // On CTRL+C - abort! //
+
+  run_flag = false;
+}
+
+int main(int argc, char * argv[])
+{
+  int      i = 0;
+  int      index;
+  int      blocks_copied;
+  int      pixy_init_status;
+  char     buf[128];
+
+  // Catch CTRL+C (SIGINT) signals //
+  signal(SIGINT, handle_SIGINT);
+
+  printf("Hello Pixy:\n libpixyusb Version: %s\n", __LIBPIXY_VERSION__);
+
+  // Connect to Pixy //
+  pixy_init_status = pixy_init();
+
+  // Was there an error initializing pixy? //
+  if(pixy_init_status != 0)
+  {
+    // Error initializing Pixy //
+    printf("pixy_init(): ");
+    pixy_error(pixy_init_status);
+
+    return pixy_init_status;
+  }
+
+  // Request Pixy firmware version //
+  {
+    uint16_t major;
+    uint16_t minor;
+    uint16_t build;
+    int      return_value;
+
+    return_value = pixy_get_firmware_version(&major, &minor, &build);
+
+    if (return_value) {
+      // Error //
+      printf("Failed to retrieve Pixy firmware version. ");
+      pixy_error(return_value);
+
+      return return_value;
+    } else {
+      // Success //
+      printf(" Pixy Firmware Version: %d.%d.%d\n", major, minor, build);
+    }
+  }
+
+#if 0
+  // Pixy Command Examples //
+  {
+    int32_t response;
+    int     return_value;
+
+    // Execute remote procedure call "cam_setAWB" with one output (host->pixy) parameter (Value = 1)
+    //
+    //   Parameters:                 Notes:
+    //
+    //   pixy_command("cam_setAWB",  String identifier for remote procedure
+    //                        0x01,  Length (in bytes) of first output parameter
+    //                           1,  Value of first output parameter
+    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
+    //                   &response,  Pointer to memory address for return value from remote procedure call
+    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
+    //
+
+    // Enable auto white balance //
+    pixy_command("cam_setAWB", UINT8(0x01), END_OUT_ARGS,  &response, END_IN_ARGS);
+
+    // Execute remote procedure call "cam_getAWB" with no output (host->pixy) parameters
+    //
+    //   Parameters:                 Notes:
+    //
+    //   pixy_command("cam_setAWB",  String identifier for remote procedure
+    //                           0,  Parameter list seperator token (See value of: END_OUT_ARGS)
+    //                   &response,  Pointer to memory address for return value from remote procedure call
+    //                           0); Parameter list seperator token (See value of: END_IN_ARGS)
+    //
+
+    // Get auto white balance //
+    return_value = pixy_command("cam_getAWB", END_OUT_ARGS, &response, END_IN_ARGS);
+
+    // Set auto white balance back to disabled //
+    pixy_command("cam_setAWB", UINT8(0x00), END_OUT_ARGS,  &response, END_IN_ARGS);
+  }
+#endif
+
+  printf("Detecting blocks...\n");
+  while(run_flag)
+  {
+
+      // Wait for new blocks to be available //
+    while(!pixy_blocks_are_new() && run_flag) {
+        pixy_service();
+    }
+
+    // Get blocks from Pixy //
+    blocks_copied = pixy_get_blocks(BLOCK_BUFFER_SIZE, &blocks[0]);
+
+    if(blocks_copied < 0) {
+      // Error: pixy_get_blocks //
+      printf("pixy_get_blocks(): ");
+      pixy_error(blocks_copied);
+    }
+
+    // Display received blocks //
+    printf("frame %d:\n", i);
+    for(index = 0; index != blocks_copied; ++index) {    
+       blocks[index].print(buf);
+       printf("  %s\n", buf);
+    }
+    i++;
+  }
+  pixy_close();
+}

discovery/libs/Pixy/src/blob.cpp.notneeded

@@ -0,0 +1,549 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+//#include <new>
+#ifdef PIXY
+#include "pixy_init.h"
+#include "exec.h"
+#else
+#include "debug.h"
+#endif
+#include "blob.h"
+
+#ifdef DEBUG
+#ifndef HOST
+#include <textdisp.h>
+#else
+#include <stdio.h>
+#endif
+
+#define DBG_BLOB(x) x
+#else
+#define DBG_BLOB(x) 
+#endif
+
+bool CBlob::recordSegments= false;
+// Set to true for testing code only.  Very slow!
+bool CBlob::testMoments= false;
+// Skip major/minor axis computation when this is false
+bool SMoments::computeAxes= false;
+int CBlob::leakcheck=0;
+
+#ifdef INCLUDE_STATS
+void SMoments::GetStats(SMomentStats &stats) const {
+    stats.area= area;
+    stats.centroidX = (float)sumX / (float)area;
+    stats.centroidY = (float)sumY / (float)area;
+
+    if (computeAxes) {
+        // Find the eigenvalues and eigenvectors for the 2x2 covariance matrix:
+        //
+        // | sum((x-|x|)^2)        sum((x-|x|)*(y-|y|)) |
+        // | sum((x-|x|)*(y-|y|))  sum((y-|y|)^2)       |
+
+        // Values= 0.5 * ((sumXX+sumYY) +- sqrt((sumXX+sumYY)^2-4(sumXXsumYY-sumXY^2)))
+        // .5 * (xx+yy) +- sqrt(xx^2+2xxyy+yy^2-4xxyy+4xy^2)
+        // .5 * (xx+yy) +- sqrt(xx^2-2xxyy+yy^2 + 4xy^2)
+
+        // sum((x-|x|)^2) =
+        // sum(x^2) - 2sum(x|x|) + sum(|x|^2) =
+        // sum(x^2) - 2|x|sum(x) + n|x|^2 =
+        // sumXX - 2*centroidX*sumX + centroidX*sumX =
+        // sumXX - centroidX*sumX
+
+        // sum((x-|x|)*(y-|y|))=
+        // sum(xy) - sum(x|y|) - sum(y|x|) + sum(|x||y|) =
+        // sum(xy) - |y|sum(x) - |x|sum(y) + n|x||y| =
+        // sumXY - centroidY*sumX - centroidX*sumY + sumX * centroidY =
+        // sumXY - centroidX*sumY
+
+        float xx= sumXX - stats.centroidX*sumX;
+        float xyTimes2= 2*(sumXY - stats.centroidX*sumY);
+        float yy= sumYY - stats.centroidY*sumY;
+        float xxMinusyy = xx-yy;
+        float xxPlusyy = xx+yy;
+        float sq = sqrt(xxMinusyy * xxMinusyy + xyTimes2*xyTimes2);
+        float eigMaxTimes2= xxPlusyy+sq;
+        float eigMinTimes2= xxPlusyy-sq;
+        stats.angle= 0.5*atan2(xyTimes2, xxMinusyy);
+        //float aspect= sqrt(eigMin/eigMax);
+        //stats.majorDiameter= sqrt(area/aspect);
+        //stats.minorDiameter= sqrt(area*aspect);
+        //
+        // sqrt(eigenvalue/area) is the standard deviation
+        // Draw the ellipse with radius of twice the standard deviation,
+        // which is a diameter of 4 times, which is 16x inside the sqrt
+
+        stats.majorDiameter= sqrt(8.0*eigMaxTimes2/area);
+        stats.minorDiameter= sqrt(8.0*eigMinTimes2/area);
+    }
+}
+
+void SSegment::GetMomentsTest(SMoments &moments) const {
+    moments.Reset();
+    int y= row;
+    for (int x= startCol; x <= endCol; x++) {
+        moments.area++;
+        moments.sumX += x;
+        moments.sumY += y;
+        if (SMoments::computeAxes) {
+            moments.sumXY += x*y;
+            moments.sumXX += x*x;
+            moments.sumYY += y*y;
+        }
+    }
+}
+#endif
+
+///////////////////////////////////////////////////////////////////////////
+// CBlob
+CBlob::CBlob() 
+{
+    DBG_BLOB(leakcheck++);
+    // Setup pointers
+    firstSegment= NULL;
+    lastSegmentPtr= &firstSegment;
+
+    // Reset blob data
+    Reset();
+}
+
+CBlob::~CBlob() 
+{
+    DBG_BLOB(leakcheck--);
+    // Free segments, if any
+    Reset();
+}
+
+void 
+CBlob::Reset() 
+{
+    // Clear blob data
+    moments.Reset();
+
+    // Empty bounds
+    right = -1;
+    left = top = 0x7fff;
+    lastBottom.row = lastBottom.invalid_row;
+    nextBottom.row = nextBottom.invalid_row;
+
+    // Delete segments if any
+    SLinkedSegment *tmp;
+    while(firstSegment!=NULL) {
+        tmp = firstSegment;
+        firstSegment = tmp->next;
+        delete tmp;
+    }
+    lastSegmentPtr= &firstSegment;
+}
+
+void 
+CBlob::NewRow() 
+{
+    if (nextBottom.row != nextBottom.invalid_row) {
+        lastBottom= nextBottom;
+        nextBottom.row= nextBottom.invalid_row;
+    }
+}
+
+void 
+CBlob::Add(const SSegment &segment) 
+{
+    // Enlarge bounding box if necessary
+    UpdateBoundingBox(segment.startCol, segment.row, segment.endCol);
+
+    // Update next attachment "surface" at bottom of blob
+    if (nextBottom.row == nextBottom.invalid_row) {
+        // New row.
+        nextBottom= segment;
+    } else {
+        // Same row.  Add to right side of nextBottom.
+        nextBottom.endCol= segment.endCol;
+    }
+    
+    SMoments segmentMoments;
+    segment.GetMoments(segmentMoments);
+    moments.Add(segmentMoments);
+
+    if (testMoments) {
+#ifdef INCLUDE_STATS
+        SMoments test;
+        segment.GetMomentsTest(test);
+        assert(test == segmentMoments);
+#endif
+    }
+    if (recordSegments) {
+        // Add segment to the _end_ of the linked list
+        *lastSegmentPtr= new /*(std::nothrow)*/ SLinkedSegment(segment);
+        if (*lastSegmentPtr==NULL)
+            return;
+        lastSegmentPtr= &((*lastSegmentPtr)->next);
+    }
+}
+
+// This takes futileResister and assimilates it into this blob
+//
+// Takes advantage of the fact that we are always assembling top to
+// bottom, left to right.
+//
+// Be sure to call like so:
+// leftblob.Assimilate(rightblob);
+//
+// This lets us assume two things:
+// 1) The assimilated blob contains no segments on the current row
+// 2) The assimilated blob lastBottom surface is to the right
+//    of this blob's lastBottom surface
+void 
+CBlob::Assimilate(CBlob &futileResister) 
+{
+    moments.Add(futileResister.moments);
+    UpdateBoundingBox(futileResister.left,
+                      futileResister.top,
+                      futileResister.right);
+    // Update lastBottom
+    if (futileResister.lastBottom.endCol > lastBottom.endCol) {
+        lastBottom.endCol= futileResister.lastBottom.endCol;
+    }
+    
+    if (recordSegments) {
+        // Take segments from futileResister, append on end
+        *lastSegmentPtr= futileResister.firstSegment;
+        lastSegmentPtr= futileResister.lastSegmentPtr;
+        futileResister.firstSegment= NULL;
+        futileResister.lastSegmentPtr= &futileResister.firstSegment;
+        // Futile resister is left with no segments
+    }
+}
+
+// Only updates left, top, and right.  bottom is updated 
+// by UpdateAttachmentSurface below
+void 
+CBlob::UpdateBoundingBox(int newLeft, int newTop, int newRight) 
+{
+    if (newLeft  < left ) left = newLeft;
+    if (newTop   < top  ) top  = newTop;
+    if (newRight > right) right= newRight;
+}
+
+///////////////////////////////////////////////////////////////////////////
+// CBlobAssembler
+
+CBlobAssembler::CBlobAssembler() 
+{
+    activeBlobs= currentBlob= finishedBlobs= NULL;
+    previousBlobPtr= &activeBlobs;
+    currentRow=-1;
+    maxRowDelta=1;
+    m_blobCount=0;
+}
+
+CBlobAssembler::~CBlobAssembler() 
+{
+    // Flush any active blobs into finished blobs
+    EndFrame();
+    // Free any finished blobs
+    Reset();
+}
+
+// Call once for each segment in the color channel
+int CBlobAssembler::Add(const SSegment &segment) {
+    if (segment.row != currentRow) {
+        // Start new row
+        currentRow= segment.row;
+        RewindCurrent();
+    }
+    
+    // Try to link this to a previous blob
+    while (currentBlob) {
+        if (segment.startCol > currentBlob->lastBottom.endCol) {
+            // Doesn't connect.  Keep searching more blobs to the right.
+            AdvanceCurrent();
+        } else {
+            if (segment.endCol < currentBlob->lastBottom.startCol) {
+                // Doesn't connect to any blob.  Stop searching.
+                break;
+            } else {
+                // Found a blob to connect to
+                currentBlob->Add(segment);
+                // Check to see if we attach to multiple blobs
+                while(currentBlob->next &&
+                      segment.endCol >= currentBlob->next->lastBottom.startCol) {
+                    // Can merge the current blob with the next one,
+                    // assimilate the next one and delete it.
+
+                    // Uncomment this for verbose output for testing
+                    // cout << "Merging blobs:" << endl
+                    //     << " curr: bottom=" << currentBlob->bottom
+                    //     << ", " << currentBlob->lastBottom.startCol
+                    //     << " to " << currentBlob->lastBottom.endCol
+                    //     << ", area " << currentBlob->moments.area << endl
+                    //     << " next: bottom=" << currentBlob->next->bottom
+                    //     << ", " << currentBlob->next->lastBottom.startCol
+                    //     << " to " << currentBlob->next->lastBottom.endCol
+                    //     << ", area " << currentBlob->next->moments.area << endl;
+
+                    CBlob *futileResister = currentBlob->next;
+                    // Cut it out of the list
+                    currentBlob->next = futileResister->next;
+                    // Assimilate it's segments and moments
+                    currentBlob->Assimilate(*(futileResister));
+
+                    // Uncomment this for verbose output for testing
+                    // cout << " NEW curr: bottom=" << currentBlob->bottom
+                    //     << ", " << currentBlob->lastBottom.startCol
+                    //     << " to " << currentBlob->lastBottom.endCol
+                    //     << ", area " << currentBlob->moments.area << endl;
+
+                    // Delete it
+                    delete futileResister;
+
+                    BlobNewRow(&currentBlob->next);
+                }
+                return 0;
+            }
+        }
+    }
+    
+    // Could not attach to previous blob, insert new one before currentBlob
+    CBlob *newBlob= new /*(std::nothrow)*/ CBlob();
+    if (newBlob==NULL)
+    {
+        DBG("blobs %d\nheap full", m_blobCount);
+        return -1;
+    }
+    m_blobCount++;
+    newBlob->next= currentBlob;
+    *previousBlobPtr= newBlob;
+    previousBlobPtr= &newBlob->next;
+    newBlob->Add(segment);
+    return 0;
+}
+
+// Call at end of frame
+// Moves all active blobs to finished list
+void CBlobAssembler::EndFrame() {
+    while (activeBlobs) {
+        activeBlobs->NewRow();
+        CBlob *tmp= activeBlobs->next;
+        activeBlobs->next= finishedBlobs;
+        finishedBlobs= activeBlobs;
+        activeBlobs= tmp;
+    }
+}
+
+int CBlobAssembler::ListLength(const CBlob *b) {
+    int len= 0;
+    while (b) {
+        len++;
+        b=b->next;
+    }
+    return len;
+}
+
+
+// Split a list of blobs into two halves
+void CBlobAssembler::SplitList(CBlob *all,
+                               CBlob *&firstHalf, CBlob *&secondHalf) {
+    firstHalf= secondHalf= all;
+    CBlob *ptr= all, **nextptr= &secondHalf;
+    while (1) {
+        if (!ptr->next) break;
+        ptr= ptr->next;
+        nextptr= &(*nextptr)->next;
+        if (!ptr->next) break;
+        ptr= ptr->next;
+    }
+    secondHalf= *nextptr;
+    *nextptr= NULL;
+}
+
+// Merge maxelts elements from old1 and old2 into newptr
+void CBlobAssembler::MergeLists(CBlob *&old1, CBlob *&old2,
+                                CBlob **&newptr, int maxelts) {
+    int n1= maxelts, n2= maxelts;
+    while (1) {
+        if (n1 && old1) {
+            if (n2 && old2 && old2->moments.area > old1->moments.area) {
+                // Choose old2
+                *newptr= old2;
+                newptr= &(*newptr)->next;
+                old2= *newptr;
+                --n2;
+            } else {
+                // Choose old1
+                *newptr= old1;
+                newptr= &(*newptr)->next;
+                old1= *newptr;
+                --n1;
+            }
+        }
+        else if (n2 && old2) {
+            // Choose old2
+            *newptr= old2;
+            newptr= &(*newptr)->next;
+            old2= *newptr;
+            --n2;
+        } else {
+            // Done
+            return;
+        }
+    }
+}
+
+#ifdef DEBUG
+void len_error() {
+    printf("len error, wedging!\n");
+    while(1);
+}
+#endif
+
+// Sorts finishedBlobs in order of descending area using an in-place
+// merge sort (time n log n)
+void CBlobAssembler::SortFinished() {
+    // Divide finishedBlobs into two lists
+    CBlob *old1, *old2;
+
+    if(finishedBlobs == NULL) {
+        return;
+    }
+
+    DBG_BLOB(int initial_len= ListLength(finishedBlobs));
+    DBG_BLOB(printf("BSort: Start 0x%x, len=%d\n", finishedBlobs,
+               initial_len));
+    SplitList(finishedBlobs, old1, old2);
+
+    // First merge lists of length 1 into sorted lists of length 2
+    // Next, merge sorted lists of length 2 into sorted lists of length 4
+    // And so on.  Terminate when only one merge is performed, which
+    // means we're completely sorted.
+    
+    for (int blocksize= 1; old2; blocksize <<= 1) {
+        CBlob *new1=NULL, *new2=NULL, **newptr1= &new1, **newptr2= &new2;
+        while (old1 || old2) {
+            DBG_BLOB(printf("BSort: o1 0x%x, o2 0x%x, bs=%d\n",
+                       old1, old2, blocksize));
+            DBG_BLOB(printf("       n1 0x%x, n2 0x%x\n",
+                       new1, new2));
+            MergeLists(old1, old2, newptr1, blocksize);
+            MergeLists(old1, old2, newptr2, blocksize);
+        }
+        *newptr1= *newptr2= NULL; // Terminate lists
+        old1= new1;
+        old2= new2;
+    }
+    finishedBlobs= old1;
+    DBG_BLOB(AssertFinishedSorted());
+    DBG_BLOB(int final_len= ListLength(finishedBlobs));
+    DBG_BLOB(printf("BSort: DONE  0x%x, len=%d\n", finishedBlobs,
+               ListLength(finishedBlobs)));
+    DBG_BLOB(if (final_len != initial_len) len_error());
+}
+
+// Assert that finishedBlobs is in fact sorted.  For testing only.
+void CBlobAssembler::AssertFinishedSorted() {
+    if (!finishedBlobs) return;
+    CBlob *i= finishedBlobs;
+    CBlob *j= i->next;
+    while (j) {
+        assert(i->moments.area >= j->moments.area);
+        i= j;
+        j= i->next;
+    }
+}
+
+void CBlobAssembler::Reset() {
+    assert(!activeBlobs);
+    currentBlob= NULL;
+    currentRow=-1;
+    m_blobCount=0;
+    while (finishedBlobs) {
+        CBlob *tmp= finishedBlobs->next;
+        delete finishedBlobs;
+        finishedBlobs= tmp;
+    }
+    DBG_BLOB(printf("after CBlobAssember::Reset, leakcheck=%d\n", CBlob::leakcheck));
+}
+
+// Manage currentBlob
+//
+// We always want to guarantee that both currentBlob
+// and currentBlob->next have had NewRow() called, and have
+// been validated to remain on the active list.  We could just
+// do this for all activeBlobs at the beginning of each row,
+// but it's less work to only do it on demand as segments come in
+// since it might allow us to skip blobs for a given row
+// if there are no segments which might overlap.
+
+// BlobNewRow:
+//
+// Tell blob there is a new row of data, and confirm that the
+// blob should still be on the active list by seeing if too many
+// rows have elapsed since the last segment was added.
+//
+// If blob should no longer be on the active list, remove it and
+// place on the finished list, and skip to the next blob.
+//
+// Call this either zero or one time per blob per row, never more.
+//
+// Pass in the pointer to the "next" field pointing to the blob, so
+// we can delete the blob from the linked list if it's not valid.
+
+void 
+CBlobAssembler::BlobNewRow(CBlob **ptr) 
+{
+    short left, top, right, bottom;
+
+    while (*ptr) {
+        CBlob *blob= *ptr;
+        blob->NewRow();
+        if (currentRow - blob->lastBottom.row > maxRowDelta) {
+            // Too many rows have elapsed.  Move it to the finished list
+            *ptr= blob->next; // cut out of current list
+            // check to see if it meets height and area constraints
+            blob->getBBox(left, top, right, bottom);
+            if (bottom-top>1) //&& blob->GetArea()>=MIN_COLOR_CODE_AREA)
+            {
+                // add to finished blobs
+                blob->next= finishedBlobs;
+                finishedBlobs= blob;
+            }
+            else
+                delete blob;
+        } else {
+            // Blob is valid
+            return;
+        }
+    }
+}
+
+void 
+CBlobAssembler::RewindCurrent() 
+{
+    BlobNewRow(&activeBlobs);
+    previousBlobPtr= &activeBlobs;
+    currentBlob= *previousBlobPtr;
+
+    if (currentBlob) BlobNewRow(&currentBlob->next);
+}
+
+void 
+CBlobAssembler::AdvanceCurrent() 
+{
+    previousBlobPtr= &(currentBlob->next);
+    currentBlob= *previousBlobPtr;
+    if (currentBlob) BlobNewRow(&currentBlob->next);
+}
+
+

discovery/libs/Pixy/src/blob.h

@@ -0,0 +1,357 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef _BLOB_H
+#define _BLOB_H
+
+// TODO
+//
+// *** Priority 1
+//
+// *** Priority 2:
+//
+// *** Priority 3:
+//
+// *** Priority 4:
+//
+// Think about heap management of CBlobs
+// Think about heap management of SLinkedSegments
+//
+// *** Priority 5 (maybe never do):
+// 
+// Try small and large SMoments structure (small for segment)
+// Try more efficient SSegment structure for lastBottom, nextBottom
+//
+// *** DONE
+//
+// DONE Compute elongation, major/minor axes (SMoments::GetStats)
+// DONE Make XRC LUT
+// DONE Use XRC LUT
+// DONE Optimize blob assy
+// DONE Start compiling
+// DONE Conditionally record segments
+// DONE Ask rich about FP, trig
+// Take segmented image in (DONE in imageserver.cc, ARW 10/7/04)
+// Produce colored segmented image out (DONE in imageserver.cc, ARW 10/7/04)
+// Draw blob stats in image out (DONE for centroid, bounding box
+//                               in imageserver.cc, ARW 10/7/04)
+// Delete segments when deleting blob (DONE, ARW 10/7/04)
+// Check to see if we attach to multiple blobs  (DONE, ARW 10/7/04)
+// Sort blobs according to area  (DONE, ARW 10/7/04)
+// DONE Sort blobs according to area
+// DONE Clean up code
+
+#include <stdlib.h>
+#include <assert.h>
+//#include <memory.h>
+#include <math.h>
+
+//#define INCLUDE_STATS
+
+// Uncomment this for verbose output for testing
+//#include <iostream.h>
+
+struct SMomentStats {
+    int   area;
+    // X is 0 on the left side of the image and increases to the right
+    // Y is 0 on the top of the image and increases to the bottom
+    float centroidX, centroidY;
+    // angle is 0 to PI, in radians.
+    // 0 points to the right (positive X)
+    // PI/2 points downward (positive Y)
+    float angle;
+    float majorDiameter;
+    float minorDiameter;
+};
+
+// Image size is 352x278
+// Full-screen blob area is 97856
+// Full-screen centroid is 176,139
+// sumX, sumY is then 17222656, 13601984; well within 32 bits
+struct SMoments {
+    // Skip major/minor axis computation when this is false
+    static bool computeAxes;
+
+    int area; // number of pixels
+    void Reset() {
+        area = 0;
+#ifdef INCLUDE_STATS
+        sumX= sumY= sumXX= sumYY= sumXY= 0;
+#endif
+    }
+#ifdef INCLUDE_STATS
+    int sumX; // sum of pixel x coords
+    int sumY; // sum of pixel y coords
+    // XX, XY, YY used for major/minor axis calculation
+    long long sumXX; // sum of x^2 for each pixel
+    long long sumYY; // sum of y^2 for each pixel
+    long long sumXY; // sum of x*y for each pixel
+#endif
+    void Add(const SMoments &moments) {
+        area += moments.area;
+#ifdef INCLUDE_STATS
+        sumX += moments.sumX;
+        sumY += moments.sumY;
+        if (computeAxes) {
+            sumXX += moments.sumXX;
+            sumYY += moments.sumYY;
+            sumXY += moments.sumXY;
+        }
+#endif
+    }
+#ifdef INCLUDE_STATS
+    void GetStats(SMomentStats &stats) const;
+    bool operator==(const SMoments &rhs) const {
+        if (area != rhs.area) return 0;
+        if (sumX != rhs.sumX) return 0;
+        if (sumY != rhs.sumY) return 0;
+        if (computeAxes) {
+            if (sumXX != rhs.sumXX) return 0;
+            if (sumYY != rhs.sumYY) return 0;
+            if (sumXY != rhs.sumXY) return 0;
+        }
+        return 1;
+    }
+#endif
+};
+
+struct SSegment {
+    unsigned char  model    : 3 ; // which color channel
+    unsigned short row      : 9 ;
+    unsigned short startCol : 10; // inclusive
+    unsigned short endCol   : 10; // inclusive
+
+    const static short invalid_row= 0x1ff;
+
+    // Sum 0^2 + 1^2 + 2^2 + ... + n^2 is (2n^3 + 3n^2 + n) / 6
+    // Sum (a+1)^2 + (a+2)^2 ... b^2 is (2(b^3-a^3) + 3(b^2-a^2) + (b-a)) / 6
+    //
+    // Sum 0+1+2+3+...+n is (n^2 + n)/2
+    // Sum (a+1) + (a+2) ... b is (b^2-a^2 + b-a)/2
+
+    void GetMoments(SMoments &moments) const {
+        int s= startCol - 1;
+        int e= endCol;
+
+        moments.area  = (e-s);
+#ifdef INCLUDE_STATS
+        int e2= e*e;
+        int y= row;
+        int s2= s*s;
+        moments.sumX = ( (e2-s2) + (e-s) ) / 2;
+        moments.sumY = (e-s) * y;
+
+        if (SMoments::computeAxes) {
+            int e3= e2*e;
+            int s3= s2*s;
+            moments.sumXY= moments.sumX*y;
+            moments.sumXX= (2*(e3-s3) + 3*(e2-s2) + (e-s)) / 6;
+            moments.sumYY= moments.sumY*y;
+        }
+#endif
+    }
+#ifdef INCLUDE_STATS
+    void GetMomentsTest(SMoments &moments) const;
+#endif
+};
+
+struct SLinkedSegment {
+    SSegment segment;
+    SLinkedSegment *next;
+    SLinkedSegment(const SSegment &segmentInit) :
+        segment(segmentInit), next(NULL) {}
+};
+
+class CBlob {
+    // These are at the beginning for fast inclusion checking
+public:
+    static int leakcheck;
+    CBlob *next;            // next ptr for linked list
+
+    // Bottom of blob, which is the surface we'll attach more segments to
+    // If bottom of blob contains multiple segments, this is the smallest
+    // segment containing the multiple segments
+    SSegment lastBottom;
+
+    // Next bottom of blob, currently under construction
+    SSegment nextBottom;
+
+    // Bounding box, inclusive.  nextBottom.row contains the "bottom"
+    short left, top, right;
+
+    void getBBox(short &leftRet, short &topRet,
+                 short &rightRet, short &bottomRet) {
+        leftRet= left;
+        topRet= top;
+        rightRet= right;
+        bottomRet= lastBottom.row;
+    }
+
+    // Segments which compose the blob
+    // Only recorded if CBlob::recordSegments is true
+    // firstSegment points to first segment in linked list
+    SLinkedSegment *firstSegment;
+    // lastSegmentPtr points to the next pointer field _inside_ the
+    // last element of the linked list.  This is the field you would
+    // modify in order to append to the end of the list.  Therefore
+    // **lastSegmentPtr should always equal to NULL.
+    // When the list is empty, lastSegmentPtr actually doesn't point inside
+    // a SLinkedSegment structure at all but instead at the firstSegment
+    // field above, which in turn is NULL.
+    SLinkedSegment **lastSegmentPtr;
+
+    SMoments moments;
+
+    static bool recordSegments;
+    // Set to true for testing code only.  Very slow!
+    static bool testMoments;
+
+    CBlob();
+    ~CBlob();
+
+    int GetArea() const {
+        return(moments.area);
+    }
+
+    // Clear blob data and free segments, if any
+    void Reset();
+    
+    void NewRow();
+
+    void Add(const SSegment &segment);
+
+    // This takes futileResister and assimilates it into this blob
+    //
+    // Takes advantage of the fact that we are always assembling top to
+    // bottom, left to right.
+    //
+    // Be sure to call like so:
+    // leftblob.Assimilate(rightblob);
+    //
+    // This lets us assume two things:
+    // 1) The assimilated blob contains no segments on the current row
+    // 2) The assimilated blob lastBottom surface is to the right
+    //    of this blob's lastBottom surface
+    void Assimilate(CBlob &futileResister);
+
+    // Only updates left, top, and right.  bottom is updated
+    // by UpdateAttachmentSurface below
+    void UpdateBoundingBox(int newLeft, int newTop, int newRight);
+};
+
+// Strategy for using CBlobAssembler:
+//
+// Make one CBlobAssembler for each color channel.
+// CBlobAssembler ignores the model index, so you need to be sure to
+// only pass the correct segments to each CBlobAssembler.
+//
+// At the beginning of a frame, call Reset() on each assembler
+// As segments appear, call Add(segment)
+// At the end of a frame, call EndFrame() on each assembler
+// Get blobs from finishedBlobs.  Blobs will remain valid until
+//    the next call to Reset(), at which point they will be deleted.
+//
+// To get statistics for a blob, do the following:
+//  SMomentStats stats;
+//  blob->moments.GetStats(stats);
+// (See imageserver.cc: draw_blob() for an example)
+
+class CBlobAssembler {
+    short currentRow;
+
+    // Active blobs, in left to right order
+    // (Active means we are still potentially adding segments)
+    CBlob *activeBlobs;
+
+    // Current candidate for adding a segment to.  This is a member
+    // of activeBlobs, and scans left to right as we search the active blobs.
+    CBlob *currentBlob;
+
+    // Pointer to pointer to current candidate, which is actually the pointer
+    // to the "next" field inside the previous candidate, or a pointer to
+    // the activeBlobs field of this object if the current candidate is the
+    // first element of the activeBlobs list.  Used for inserting and
+    // deleting blobs.
+    CBlob **previousBlobPtr;
+
+public:
+    // Blobs we're no longer adding to
+    CBlob *finishedBlobs;
+    short maxRowDelta;
+    static bool keepFinishedSorted;
+
+public:
+    CBlobAssembler();
+    ~CBlobAssembler();
+
+    // Call prior to starting a frame
+    // Deletes any previously created blobs
+    void Reset();
+
+
+    // Call once for each segment in the color channel
+    int Add(const SSegment &segment);
+
+    // Call at end of frame
+    // Moves all active blobs to finished list
+    void EndFrame();
+
+    int ListLength(const CBlob *b);
+    
+    // Split a list of blobs into two halves
+    void SplitList(CBlob *all, CBlob *&firstHalf, CBlob *&secondHalf);
+
+    // Merge maxelts elements from old1 and old2 into newptr
+    void MergeLists(CBlob *&old1, CBlob *&old2, CBlob **&newptr, int maxelts);
+
+    // Sorts finishedBlobs in order of descending area using an in-place
+    // merge sort (time n log n)
+    void SortFinished();
+
+    // Assert that finishedBlobs is in fact sorted.  For testing only.
+    void AssertFinishedSorted();
+
+protected:
+    // Manage currentBlob
+    //
+    // We always want to guarantee that both currentBlob
+    // and currentBlob->next have had NewRow() called, and have
+    // been validated to remain on the active list.  We could just
+    // do this for all activeBlobs at the beginning of each row,
+    // but it's less work to only do it on demand as segments come in
+    // since it might allow us to skip blobs for a given row
+    // if there are no segments which might overlap.
+
+    // BlobNewRow:
+    //
+    // Tell blob there is a new row of data, and confirm that the
+    // blob should still be on the active list by seeing if too many
+    // rows have elapsed since the last segment was added.
+    //
+    // If blob should no longer be on the active list, remove it and
+    // place on the finished list, and skip to the next blob.
+    //
+    // Call this either zero or one time per blob per row, never more.
+    //
+    // Pass in the pointer to the "next" field pointing to the blob, so
+    // we can delete the blob from the linked list if it's not valid.
+
+    void BlobNewRow(CBlob **ptr);
+    void RewindCurrent();
+    void AdvanceCurrent();
+
+    int m_blobCount;
+};
+
+#endif // _BLOB_H

discovery/libs/Pixy/src/blobs.h

@@ -0,0 +1,111 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef BLOBS_H
+#define BLOBS_H
+
+#include <stdint.h>
+#include "blob.h"
+#include "pixytypes.h"
+#include "colorlut.h"
+#include "qqueue.h"
+
+#define MAX_BLOBS             100
+#define MAX_BLOBS_PER_MODEL   20
+#define MAX_MERGE_DIST        5
+#define MIN_AREA              20
+#define MIN_COLOR_CODE_AREA   10
+#define MAX_CODED_DIST        6
+#define MAX_COLOR_CODE_MODELS 5
+
+#define BL_BEGIN_MARKER	      0xaa55
+#define BL_BEGIN_MARKER_CC    0xaa56
+
+enum ColorCodeMode
+{
+    DISABLED = 0,
+    ENABLED = 1,
+    CC_ONLY = 2,
+    MIXED = 3 // experimental
+};
+
+class Blobs
+{
+public:
+    Blobs(Qqueue *qq, uint8_t *lut);
+    ~Blobs();
+    int blobify();
+    uint16_t getBlock(uint8_t *buf, uint32_t buflen);
+    uint16_t getCCBlock(uint8_t *buf, uint32_t buflen);
+    BlobA *getMaxBlob(uint16_t signature=0);
+    void getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen);
+    int setParams(uint16_t maxBlobs, uint16_t maxBlobsPerModel, uint32_t minArea, ColorCodeMode ccMode);
+    int runlengthAnalysis();
+#ifndef PIXY
+    void getRunlengths(uint32_t **qvals, uint32_t *len);
+#endif
+
+	ColorLUT m_clut;
+    Qqueue *m_qq;
+
+private:
+    int handleSegment(uint8_t signature, uint16_t row, uint16_t startCol, uint16_t length);
+	void endFrame();
+    uint16_t combine(uint16_t *blobs, uint16_t numBlobs);
+    uint16_t combine2(uint16_t *blobs, uint16_t numBlobs);
+    uint16_t compress(uint16_t *blobs, uint16_t numBlobs);
+
+    bool closeby(BlobA *blob0, BlobA *blob1);
+    int16_t distance(BlobA *blob0, BlobA *blob1);
+    void sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz);
+    int16_t angle(BlobA *blob0, BlobA *blob1);
+    int16_t distance(BlobA *blob0, BlobA *blob1, bool horiz);
+    void processCC();
+    void cleanup(BlobA *blobs[], int16_t *numBlobs);
+    void cleanup2(BlobA *blobs[], int16_t *numBlobs);
+    bool analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB);
+    void mergeClumps(uint16_t scount0, uint16_t scount1);
+
+    void printBlobs();
+
+    CBlobAssembler m_assembler[CL_NUM_SIGNATURES];
+
+    uint16_t *m_blobs;
+    uint16_t m_numBlobs;
+
+    BlobB *m_ccBlobs;
+    uint16_t m_numCCBlobs;
+
+    bool m_mutex;
+    uint16_t m_maxBlobs;
+    uint16_t m_maxBlobsPerModel;
+
+    uint16_t m_blobReadIndex;
+    uint16_t m_ccBlobReadIndex;
+
+    uint32_t m_minArea;
+    uint16_t m_mergeDist;
+    uint16_t m_maxCodedDist;
+    ColorCodeMode m_ccMode;
+    BlobA *m_maxBlob;
+
+#ifndef PIXY
+    uint32_t m_numQvals;
+    uint32_t *m_qvals;
+#endif
+};
+
+
+
+#endif // BLOBS_H

discovery/libs/Pixy/src/calc.cpp.notneeded

@@ -0,0 +1,108 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include "calc.h"
+
+void hsvc(uint8_t r, uint8_t g, uint8_t b, uint8_t *h, uint8_t *s, uint8_t *v, uint8_t *c)
+{
+    uint8_t min, max, delta;
+    int hue;
+    min = MIN(r, g);
+    min = MIN(min, b);
+    max = MAX(r, g);
+    max = MAX(max, b);
+
+    *v = max;
+    delta = max - min;
+    if (max>50)
+    {
+        //if (delta>50)
+            *s = ((int)delta<<8)/max;
+        //else
+        //    *s = 0;
+    }
+    else
+        *s = 0;
+    if (max==0 || delta==0)
+    {
+        *s = 0;
+        *h = 0;
+        *c = 0;
+        return;
+    }
+    if (r==max)
+        hue = (((int)g - (int)b)<<8)/delta;         // between yellow & magenta
+    else if (g==max)
+        hue = (2<<8) + (((int)b - (int)r)<<8)/delta;     // between cyan & yellow
+    else
+        hue = (4<<8) + (((int)r - (int)g)<<8)/delta;     // between magenta & cyan
+    if(hue < 0)
+        hue += 6<<8;
+    hue /= 6;
+    *h = hue;
+    *c = delta;
+}
+
+uint32_t lighten(uint32_t color, uint8_t factor)
+{
+    uint32_t r, g, b;
+
+    rgbUnpack(color, &r, &g, &b);
+
+    r += factor;
+    g += factor;
+    b += factor;
+
+    return rgbPack(r, g, b);
+}
+
+uint32_t rgbPack(uint32_t r, uint32_t g, uint32_t b)
+{
+    if (r>0xff)
+        r = 0xff;
+    if (g>0xff)
+        g = 0xff;
+    if (b>0xff)
+        b = 0xff;
+    return (r<<16) | (g<<8) | b;
+}
+
+void rgbUnpack(uint32_t color, uint32_t *r, uint32_t *g, uint32_t *b)
+{
+    *b = color&0xff;
+    color >>= 8;
+    *g = color&0xff;
+    color >>= 8;
+    *r = color&0xff;
+}
+
+uint32_t saturate(uint32_t color)
+{
+    float m;
+    uint32_t max, r, g, b;
+
+    rgbUnpack(color, &r, &g, &b);
+
+    max = MAX(r, g);
+    max = MAX(max, b);
+
+    // saturate while maintaining ratios
+    m = 255.0f/max;
+    r = (uint8_t)(m*r);
+    g = (uint8_t)(m*g);
+    b = (uint8_t)(m*b);
+
+    return rgbPack(r, g, b);
+}

discovery/libs/Pixy/src/calc.h

@@ -0,0 +1,35 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef CALC_H
+#define CALC_H
+#include <inttypes.h>
+
+#ifdef MAX
+#undef MAX
+#endif
+#ifdef MIN
+#undef MIN
+#endif
+#define MAX(a, b)  (a>b ? a : b)
+#define MIN(a, b)  (a<b ? a : b)
+
+void hsvc(uint8_t r, uint8_t g, uint8_t b, uint8_t *h, uint8_t *s, uint8_t *v, uint8_t *c);
+uint32_t lighten(uint32_t color, uint8_t factor);
+uint32_t saturate(uint32_t color);
+uint32_t rgbPack(uint32_t r, uint32_t g, uint32_t b);
+void rgbUnpack(uint32_t color, uint32_t *r, uint32_t *g, uint32_t *b);
+
+#endif // CALC_H

discovery/libs/Pixy/src/chirp.hpp

@@ -0,0 +1,291 @@
+#ifndef CHIRP_HPP
+#define CHIRP_HPP
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include "link.h"
+
+#define ALIGN(v, n)  v = v&((n)-1) ? (v&~((n)-1))+(n) : v
+#define FOURCC(a, b, c, d)  (((uint32_t)a<<0)|((uint32_t)b<<8)|((uint32_t)c<<16)|((uint32_t)d<<24))
+
+#define CRP_RES_OK                      0
+#define CRP_RES_ERROR                   -1
+#define CRP_RES_ERROR_RECV_TIMEOUT      LINK_RESULT_ERROR_RECV_TIMEOUT
+#define CRP_RES_ERROR_SEND_TIMEOUT      LINK_RESULT_ERROR_SEND_TIMEOUT
+#define CRP_RES_ERROR_CRC               -2
+#define CRP_RES_ERROR_PARSE             -3
+#define CRP_RES_ERROR_MAX_NAK           -4
+#define CRP_RES_ERROR_MEMORY            -5
+#define CRP_RES_ERROR_NOT_CONNECTED     -6
+
+#define CRP_MAX_NAK                     3
+#define CRP_RETRIES                     3
+#define CRP_HEADER_TIMEOUT    	        1000
+#define CRP_DATA_TIMEOUT                500
+#define CRP_IDLE_TIMEOUT                500
+#define CRP_SEND_TIMEOUT                1000
+#define CRP_MAX_ARGS                    10
+#define CRP_BUFSIZE                     0x80
+#define CRP_BUFPAD                      8
+#define CRP_PROCTABLE_LEN               0x40
+
+#define CRP_START_CODE                  0xaaaa5555
+
+#define CRP_CALL                        0x80
+#define CRP_RESPONSE                    0x40
+#define CRP_INTRINSIC                   0x20
+#define CRP_DATA                        0x10
+#define CRP_XDATA                       0x18 // data not associated with no associated procedure)
+#define CRP_CALL_ENUMERATE              (CRP_CALL | CRP_INTRINSIC | 0x00)
+#define CRP_CALL_INIT                   (CRP_CALL | CRP_INTRINSIC | 0x01)
+#define CRP_CALL_ENUMERATE_INFO         (CRP_CALL | CRP_INTRINSIC | 0x02)
+
+#define CRP_ACK                         0x59
+#define CRP_NACK                        0x95
+#define CRP_MAX_HEADER_LEN              64
+
+#define CRP_ARRAY                       0x80 // bit
+#define CRP_FLT                         0x10 // bit
+#define CRP_NO_COPY                     (0x10 | 0x20)
+#define CRP_HINT                        0x40 // bit
+#define CRP_NULLTERM_ARRAY              (0x20 | CRP_ARRAY) // bits
+#define CRP_INT8                        0x01
+#define CRP_UINT8                       0x01
+#define CRP_INT16                       0x02
+#define CRP_UINT16                      0x02
+#define CRP_INT32                       0x04
+#define CRP_UINT32                      0x04
+#define CRP_FLT32                       (CRP_FLT | 0x04)
+#define CRP_FLT64                       (CRP_FLT | 0x08)
+#define CRP_STRING                      (CRP_NULLTERM_ARRAY | CRP_INT8)
+#define CRP_TYPE_HINT                   0x64 // type hint identifier
+#define CRP_INTS8                       (CRP_INT8 | CRP_ARRAY)
+#define CRP_INTS16                      (CRP_INT16 | CRP_ARRAY)
+#define CRP_INTS32                      (CRP_INT32 | CRP_ARRAY)
+#define CRP_UINTS8                      CRP_INTS8
+#define CRP_UINTS8_NO_COPY              (CRP_INTS8 | CRP_NO_COPY)
+#define CRP_UINTS16_NO_COPY             (CRP_INTS16 | CRP_NO_COPY)
+#define CRP_UINTS32_NO_COPY             (CRP_INTS32 | CRP_NO_COPY)
+#define CRP_UINTS16                     CRP_INTS16
+#define CRP_UINTS32                     CRP_INTS32
+#define CRP_FLTS32                      (CRP_FLT32 | CRP_ARRAY)
+#define CRP_FLTS64                      (CRP_FLT64 | CRP_ARRAY)
+#define CRP_HINT8                       (CRP_INT8 | CRP_HINT)
+#define CRP_HINT16                      (CRP_INT16 | CRP_HINT)
+#define CRP_HINT32                      (CRP_INT32 | CRP_HINT)
+#define CRP_HINTS8                      (CRP_INT8 | CRP_ARRAY | CRP_HINT)
+#define CRP_HINTS16                     (CRP_INT16 | CRP_ARRAY | CRP_HINT)
+#define CRP_HINTS32                     (CRP_INT32 | CRP_ARRAY | CRP_HINT)
+#define CRP_HFLTS32                     (CRP_FLT32 | CRP_ARRAY | CRP_HINT)
+#define CRP_HFLTS64                     (CRP_FLT64 | CRP_ARRAY | CRP_HINT)
+#define CRP_HSTRING                     (CRP_STRING | CRP_HINT)
+// CRP_HTYPE is for arg lists which are uint8_t arrays
+#define CRP_HTYPE(v)                    CRP_TYPE_HINT, (uint8_t)(v>>0&0xff), (uint8_t)(v>>8&0xff), (uint8_t)(v>>16&0xff), (uint8_t)(v>>24&0xff)
+
+// regular call args
+#define INT8(v)                         CRP_INT8, v
+#define UINT8(v)                        CRP_INT8, v
+#define INT16(v)                        CRP_INT16, v
+#define UINT16(v)                       CRP_INT16, v
+#define INT32(v)                        CRP_INT32, v
+#define UINT32(v)                       CRP_INT32, v
+#define FLT32(v)                        CRP_FLT32, v
+#define FLT64(v)                        CRP_FLT64, v
+#define STRING(s)                       CRP_STRING, s
+#define INTS8(len, a)                   CRP_INTS8, len, a
+#define UINTS8(len, a)                  CRP_INTS8, len, a
+#define UINTS8_NO_COPY(len)             CRP_UINTS8_NO_COPY, len
+#define UINTS16_NO_COPY(len)            CRP_UINTS16_NO_COPY, len
+#define UINTS32_NO_COPY(len)            CRP_UINTS32_NO_COPY, len
+#define INTS16(len, a)                  CRP_INTS16, len, a
+#define UINTS16(len, a)                 CRP_INTS16, len, a
+#define INTS32(len, a)                  CRP_INTS32, len, a
+#define UINTS32(len, a)                 CRP_INTS32, len, a
+#define FLTS32(len, a)                  CRP_FLTS32, len, a
+#define FLTS64(len, a)                  CRP_FLTS64, len, a
+
+// hint call args
+#define HINT8(v)                        CRP_HINT8, v
+#define UHINT8(v)                       CRP_HINT8, v
+#define HINT16(v)                       CRP_HINT16, v
+#define UHINT16(v)                      CRP_HINT16, v
+#define HINT32(v)                       CRP_HINT32, v
+#define UHINT32(v)                      CRP_HINT32, v
+#define HFLT32(v)                       CRP_HFLT32, v
+#define HFLT64(v)                       CRP_HFLT64, v
+#define HSTRING(s)                      CRP_HSTRING, s
+#define HINTS8(len, a)                  CRP_HINTS8, len, a
+#define UHINTS8(len, a)                 CRP_HINTS8, len, a
+#define HINTS16(len, a)                 CRP_HINTS16, len, a
+#define UHINTS16(len, a)                CRP_HINTS16, len, a
+#define HINTS32(len, a)                 CRP_HINTS32, len, a
+#define UHINTS32(len, a)                CRP_HINTS32, len, a
+#define HFLTS32(len, a)                 CRP_HFLTS32, len, a
+#define HFLTS64(len, a)                 CRP_HFLTS64, len, a
+#define HTYPE(v)                        CRP_TYPE_HINT, v
+
+#define INT8_IN(v)                      int8_t & v
+#define UINT8_IN(v)                     uint8_t & v
+#define INT16_IN(v)                     int16_t & v
+#define UINT16_IN(v)                    uint16_t & v
+#define INT32_IN(v)                     int32_t & v
+#define UINT32_IN(v)                    uint32_t & v
+#define FLT32_IN(v)                     float & v
+#define FLT64_IN(v)                     double & v
+#define STRING_IN(s)                    const char * s
+#define INTS8_IN(len, a)                uint32_t & len, int8_t * a
+#define UINTS8_IN(len, a)               uint32_t & len, uint8_t * a
+#define INTS16_IN(len, a)               uint32_t & len, int16_t * a
+#define UINTS16_IN(len, a)              uint32_t & len, uint16_t * a
+#define INTS32_IN(len, a)               uint32_t & len, int32_t * a
+#define UINTS32_IN(len, a)              uint32_t & len, uint32_t * a
+#define FLTS32_IN(len, a)               uint32_t & len, float * a
+#define FLTS64_IN(len, a)               uint32_t & len, double * a
+
+#ifndef END
+#ifdef __x86_64__
+#define END                             (int64_t)0
+#else
+#define END                             0
+#endif
+#endif
+#define END_OUT_ARGS                    END
+#define END_IN_ARGS                     END
+
+// service types
+#define SYNC                            0
+#define ASYNC                           0x01 // bit
+#define RETURN_ARRAY                    0x02 // bit
+#define SYNC_RETURN_ARRAY               (SYNC | RETURN_ARRAY)
+
+#define CRP_RETURN(chirp, ...)          chirp->assemble(0, __VA_ARGS__, END)
+#define CRP_SEND_XDATA(chirp, ...)      chirp->assemble(CRP_XDATA, __VA_ARGS__, END)
+#define callSync(...)                   call(SYNC, __VA_ARGS__, END)
+#define callAsync(...)                  call(ASYNC, __VA_ARGS__, END)
+#define callSyncArray(...)              call(SYNC_RETURN_ARRAY, __VA_ARGS__, END)
+
+class Chirp;
+
+typedef int16_t ChirpProc; // negative values are invalid
+
+typedef uint32_t (*ProcPtr)(Chirp *);
+
+struct ProcModule
+{
+    char *procName;
+    ProcPtr procPtr;
+    uint8_t argTypes[CRP_MAX_ARGS];
+    char *procInfo;
+};
+
+struct ProcTableExtension
+{
+    uint8_t argTypes[CRP_MAX_ARGS];
+    char *procInfo;
+};
+
+struct ProcInfo
+{
+    char *procName;
+    uint8_t *argTypes;
+    char *procInfo;
+};
+
+struct ProcTableEntry
+{
+    const char *procName;
+    ProcPtr procPtr;
+    ChirpProc chirpProc;
+    const ProcTableExtension *extension;
+};
+
+class Chirp
+{
+public:
+    Chirp(bool hinterested=false, bool client=false, Link *link=NULL);
+    ~Chirp();
+
+    virtual int init(bool connect);
+    int setLink(Link *link);
+    ChirpProc getProc(const char *procName, ProcPtr callback=0);
+    int setProc(const char *procName, ProcPtr proc,  ProcTableExtension *extension=NULL);
+    int getProcInfo(ChirpProc proc, ProcInfo *info);
+    int registerModule(const ProcModule *module);
+    void setSendTimeout(uint32_t timeout);
+    void setRecvTimeout(uint32_t timeout);
+
+    int call(uint8_t service, ChirpProc proc, ...);
+    int call(uint8_t service, ChirpProc proc, va_list args);
+    static uint8_t getType(const void *arg);
+    int service(bool all=true);
+    int assemble(uint8_t type, ...);
+    bool connected();
+
+    // utility methods
+    static int serialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, ...);
+    static int deserialize(uint8_t *buf, uint32_t len, ...);
+    static int vserialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, va_list *args);
+    static int vdeserialize(uint8_t *buf, uint32_t len, va_list *args);
+    static int deserializeParse(uint8_t *buf, uint32_t len, void *args[]);
+    static int loadArgs(va_list *args, void *recvArgs[]);
+    static int getArgList(uint8_t *buf, uint32_t len, uint8_t *argList);
+    int useBuffer(uint8_t *buf, uint32_t len);
+
+    static uint16_t calcCrc(uint8_t *buf, uint32_t len);
+
+protected:
+    int remoteInit(bool connect);
+    int recvChirp(uint8_t *type, ChirpProc *proc, void *args[], bool wait=false); // null pointer terminates
+    virtual int handleChirp(uint8_t type, ChirpProc proc, const void *args[]); // null pointer terminates
+    virtual void handleXdata(const void *data[]) {(void)data;}
+    virtual int sendChirp(uint8_t type, ChirpProc proc);
+
+    uint8_t *m_buf;
+    uint8_t *m_bufSave;
+    uint32_t m_len;
+    uint32_t m_offset;
+    uint32_t m_bufSize;
+    bool m_errorCorrected;
+    bool m_sharedMem;
+    bool m_hinformer;
+    bool m_hinterested;
+    bool m_client;
+    uint32_t m_headerLen;
+    uint16_t m_headerTimeout;
+    uint16_t m_dataTimeout;
+    uint16_t m_idleTimeout;
+    uint16_t m_sendTimeout;
+
+private:
+    int sendHeader(uint8_t type, ChirpProc proc);
+    int sendFull(uint8_t type, ChirpProc proc);
+    int sendData();
+    int sendAck(bool ack); // false=nack
+    int sendChirpRetry(uint8_t type, ChirpProc proc);
+    int recvHeader(uint8_t *type, ChirpProc *proc, bool wait);
+    int recvFull(uint8_t *type, ChirpProc *proc, bool wait);
+    int recvData();
+    int recvAck(bool *ack, uint16_t timeout); // false=nack
+    int32_t handleEnumerate(char *procName, ChirpProc *callback);
+    int32_t handleInit(uint16_t *blkSize, uint8_t *hintSource);
+    int32_t handleEnumerateInfo(ChirpProc *proc);
+    int vassemble(va_list *args);
+    void restoreBuffer();
+
+    ChirpProc updateTable(const char *procName, ProcPtr procPtr);
+    ChirpProc lookupTable(const char *procName);
+    int realloc(uint32_t min=0);
+    int reallocTable();
+
+    Link *m_link;
+    ProcTableEntry *m_procTable;
+    uint16_t m_procTableSize;
+    uint16_t m_blkSize;
+    uint8_t m_maxNak;
+    uint8_t m_retries;
+    bool m_call;
+    bool m_connected;
+};
+
+#endif // CHIRP_H

discovery/libs/Pixy/src/chirpreceiver.cpp

@@ -0,0 +1,38 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include "chirpreceiver.hpp"
+
+ChirpReceiver::ChirpReceiver(USBLink * link, Interpreter * interpreter)
+{
+  m_hinterested = true;
+  m_client      = true;
+  interpreter_  = interpreter;
+
+  setLink(link);
+}
+
+ChirpReceiver::~ChirpReceiver()
+{
+  // This destructor does nothing but is necessary  //
+  // for successful linkage on some combinations of //
+  // compilers and platforms.                       //
+}
+
+void ChirpReceiver::handleXdata(const void * data[])
+{
+  // Interpret (Chirp) messages from Pixy //
+  interpreter_->interpret_data(data);
+}

discovery/libs/Pixy/src/chirpreceiver.hpp

@@ -0,0 +1,43 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef __CHIRPRECEIVER_HPP__
+#define __CHIRPRECEIVER_HPP__
+
+#include "chirp.hpp"
+#include "usblink.h"
+#include "interpreter.hpp"
+
+class ChirpReceiver : public Chirp
+{
+  public:
+
+    ChirpReceiver(USBLink * link, Interpreter * interpreter);
+    virtual ~ChirpReceiver();
+
+  private:
+
+    Interpreter * interpreter_;
+
+    /**
+      @brief Called by Chrip::service() when data
+             is received from Pixy. 
+
+      @param[in] data  Incoming Chirp protocol data from Pixy.
+    */
+    void handleXdata(const void * data[]);
+};
+
+#endif

discovery/libs/Pixy/src/colorlut.cpp.notneeded

@@ -0,0 +1,642 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+#ifndef PIXY
+#include "debug.h"
+#endif
+#include "colorlut.h"
+#include "calc.h"
+
+
+
+IterPixel::IterPixel(const Frame8 &frame, const RectA &region)
+{
+    m_frame = frame;
+    m_region = region;
+    m_points = NULL;
+    reset();
+}
+
+IterPixel::IterPixel(const Frame8 &frame, const Points *points)
+{
+    m_frame = frame;
+    m_points = points;
+    reset();
+}
+
+bool IterPixel::reset(bool cleari)
+{
+    if (cleari)
+        m_i = 0;
+    if (m_points)
+    {
+        if (m_points->size()>m_i)
+        {
+            m_region = RectA((*m_points)[m_i].m_x, (*m_points)[m_i].m_y, CL_GROW_INC, CL_GROW_INC);
+            m_i++;
+        }
+        else
+            return false; // empty!
+    }
+    m_x = m_y = 0;
+    m_pixels = m_frame.m_pixels + (m_region.m_yOffset | 1)*m_frame.m_width + (m_region.m_xOffset | 1);
+    return true;
+}
+
+bool IterPixel::next(UVPixel *uv, RGBPixel *rgb)
+{
+    if (m_points)
+    {
+        if (nextHelper(uv, rgb))
+            return true; // working on the current block
+        else // get new block
+        {
+            if (reset(false)) // reset indexes, increment m_i, get new block
+                return nextHelper(uv, rgb);  // we have another block!
+            else
+                return false; // blocks are empty
+        }
+    }
+    else
+        return nextHelper(uv, rgb);
+}
+
+
+bool IterPixel::nextHelper(UVPixel *uv, RGBPixel *rgb)
+{
+    int32_t r, g1, g2, b, u, v, c, miny=CL_MIN_Y;
+
+    while(1)
+    {
+        if (m_x>=m_region.m_width)
+        {
+            m_x = 0;
+            m_y += 2;
+            m_pixels += m_frame.m_width*2;
+        }
+        if (m_y>=m_region.m_height)
+            return false;
+
+        r = m_pixels[m_x];
+        g1 = m_pixels[m_x - 1];
+        g2 = m_pixels[-m_frame.m_width + m_x];
+        b = m_pixels[-m_frame.m_width + m_x - 1];
+		if (rgb)
+		{
+		  	rgb->m_r = r;
+			rgb->m_g = (g1+g2)/2;
+			rgb->m_b = b;
+		}
+		if (uv)
+		{
+        	c = r+g1+b;
+            if (c<miny)
+			{
+				m_x += 2;
+            	continue;
+			}
+        	u = ((r-g1)<<CL_LUT_ENTRY_SCALE)/c;
+        	c = r+g2+b;
+            if (c<miny)
+			{
+				m_x += 2;
+            	continue;
+			}
+        	v = ((b-g2)<<CL_LUT_ENTRY_SCALE)/c;
+
+        	uv->m_u = u;
+        	uv->m_v = v;
+		}
+
+		m_x += 2;
+        return true;
+    }
+}
+
+uint32_t IterPixel::averageRgb(uint32_t *pixels)
+{
+	RGBPixel rgb;
+	uint32_t r, g, b, n;
+	reset();
+	for (r=g=b=n=0; next(NULL, &rgb); n++)
+	{
+		r += rgb.m_r;
+		g += rgb.m_g;
+		b += rgb.m_b;		
+	}
+
+	r /= n;
+	g /= n;
+	b /= n;
+
+	if (pixels)
+		*pixels = n;
+	return (r<<16) | (g<<8) | b;
+}
+
+ColorLUT::ColorLUT(uint8_t *lut)
+{
+	int i; 
+    m_lut = lut;
+    memset((void *)m_signatures, 0, sizeof(ColorSignature)*CL_NUM_SIGNATURES);
+    memset((void *)m_runtimeSigs, 0, sizeof(RuntimeSignature)*CL_NUM_SIGNATURES);
+	clearLUT();
+
+    setMinBrightness(CL_DEFAULT_MINY);
+    m_minRatio = CL_MIN_RATIO;
+    m_maxDist = CL_MAX_DIST;
+    m_ratio = CL_DEFAULT_TOL;
+    m_ccGain = CL_DEFAULT_CCGAIN;
+	for (i=0; i<CL_NUM_SIGNATURES; i++)
+		m_sigRanges[i] = CL_DEFAULT_SIG_RANGE;
+}
+
+
+ColorLUT::~ColorLUT()
+{
+}
+
+
+void ColorLUT::calcRatios(IterPixel *ip, ColorSignature *sig, float ratios[])
+{
+    UVPixel uv;
+    uint32_t n=0, counts[4];
+    longlong usum=0, vsum=0;
+    counts[0] = counts[1] = counts[2] = counts[3] = 0;
+
+    ip->reset();
+    while(ip->next(&uv))
+    {
+        if (uv.m_u>sig->m_uMin)
+            counts[0]++;
+
+        if (uv.m_u<sig->m_uMax)
+            counts[1]++;
+
+        if (uv.m_v>sig->m_vMin)
+            counts[2]++;
+
+        if (uv.m_v<sig->m_vMax)
+            counts[3]++;
+
+        usum += uv.m_u;
+        vsum += uv.m_v;
+        n++;
+    }
+
+    // calc ratios
+    ratios[0] = (float)counts[0]/n;
+    ratios[1] = (float)counts[1]/n;
+    ratios[2] = (float)counts[2]/n;
+    ratios[3] = (float)counts[3]/n;
+    // calc mean (because it's cheap to do it here)
+    sig->m_uMean = usum/n;
+    sig->m_vMean = vsum/n;
+}
+
+
+void ColorLUT::iterate(IterPixel *ip, ColorSignature *sig)
+{
+    int32_t scale;
+    float ratios[4];
+
+    // binary search -- this rouine is guaranteed to find the right value +/- 1, which is good enough!
+    // find all four values, umin, umax, vmin, vmax simultaneously
+    for (scale=1<<30, sig->m_uMin=sig->m_uMax=sig->m_vMin=sig->m_vMax=0; scale!=0; scale>>=1)
+    {
+        calcRatios(ip, sig, ratios);
+        if (ratios[0]>m_ratio)
+            sig->m_uMin += scale;
+        else
+            sig->m_uMin -= scale;
+
+        if (ratios[1]>m_ratio)
+            sig->m_uMax -= scale;
+        else
+            sig->m_uMax += scale;
+
+        if (ratios[2]>m_ratio)
+            sig->m_vMin += scale;
+        else
+            sig->m_vMin -= scale;
+
+        if (ratios[3]>m_ratio)
+            sig->m_vMax -= scale;
+        else
+            sig->m_vMax += scale;
+    }
+}
+
+
+
+
+int ColorLUT::generateSignature(const Frame8 &frame, const RectA &region, uint8_t signum)
+{
+ 	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return -1;
+   // this is cool-- this routine doesn't allocate any extra memory other than some stack variables
+    IterPixel ip(frame, region);
+    iterate(&ip, m_signatures+signum-1);
+	m_signatures[signum-1].m_type = 0;
+
+	updateSignature(signum);
+    return 0;
+}
+
+
+int ColorLUT::generateSignature(const Frame8 &frame, const Point16 &point, Points *points, uint8_t signum)
+{
+	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return -1;
+    // this routine requires some memory to store the region which consists of some consistently-sized blocks
+    growRegion(frame, point, points);
+    IterPixel ip(frame, points);
+    iterate(&ip, m_signatures+signum-1);
+	m_signatures[signum-1].m_type = 0;
+
+	updateSignature(signum);
+    return 0;
+}
+
+void ColorLUT::updateSignature(uint8_t signum)
+{
+    float range;
+
+	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return;
+	signum--;
+
+    if (m_signatures[signum].m_type==CL_MODEL_TYPE_COLORCODE)
+        range = m_sigRanges[signum]*m_ccGain;
+	else
+		range = m_sigRanges[signum];
+    m_runtimeSigs[signum].m_uMin = m_signatures[signum].m_uMean + (m_signatures[signum].m_uMin - m_signatures[signum].m_uMean)*range;
+	m_runtimeSigs[signum].m_uMax = m_signatures[signum].m_uMean + (m_signatures[signum].m_uMax - m_signatures[signum].m_uMean)*range;
+	m_runtimeSigs[signum].m_vMin = m_signatures[signum].m_vMean + (m_signatures[signum].m_vMin - m_signatures[signum].m_vMean)*range;
+	m_runtimeSigs[signum].m_vMax = m_signatures[signum].m_vMean + (m_signatures[signum].m_vMax - m_signatures[signum].m_vMean)*range;
+
+    m_runtimeSigs[signum].m_rgbSat = saturate(m_signatures[signum].m_rgb);
+}
+
+ColorSignature *ColorLUT::getSignature(uint8_t signum)
+{
+	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return NULL;
+
+	return m_signatures+signum-1;
+}
+
+int ColorLUT::setSignature(uint8_t signum, const ColorSignature &sig)
+{
+	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return -1;
+
+	m_signatures[signum-1] = sig;
+	updateSignature(signum);
+	return 0;
+}
+
+
+int ColorLUT::generateLUT()
+{
+    int32_t r, g, b, u, v, y, bin, sig;
+
+    clearLUT();
+
+    // recalc bounds for each signature
+    for (r=0; r<CL_NUM_SIGNATURES; r++)
+        updateSignature(r+1);
+
+    for (r=0; r<1<<8; r+=1<<(8-CL_LUT_COMPONENT_SCALE))
+    {
+        for (g=0; g<1<<8; g+=1<<(8-CL_LUT_COMPONENT_SCALE))
+        {
+            for (b=0; b<1<<8; b+=1<<(8-CL_LUT_COMPONENT_SCALE))
+            {
+                y = r+g+b;
+
+                if (y<(int32_t)m_miny)
+                    continue;
+                u = ((r-g)<<CL_LUT_ENTRY_SCALE)/y;
+                v = ((b-g)<<CL_LUT_ENTRY_SCALE)/y;
+
+                for (sig=0; sig<CL_NUM_SIGNATURES; sig++)
+                {
+                    if (m_signatures[sig].m_uMin==0 && m_signatures[sig].m_uMax==0)
+                        continue;
+                    if ((m_runtimeSigs[sig].m_uMin<u) && (u<m_runtimeSigs[sig].m_uMax) &&
+                            (m_runtimeSigs[sig].m_vMin<v) && (v<m_runtimeSigs[sig].m_vMax))
+                    {
+                        u = r-g;
+                        u >>= 9-CL_LUT_COMPONENT_SCALE;
+                        u &= (1<<CL_LUT_COMPONENT_SCALE)-1;
+                        v = b-g;
+                        v >>= 9-CL_LUT_COMPONENT_SCALE;
+                        v &= (1<<CL_LUT_COMPONENT_SCALE)-1;
+
+                        bin = (u<<CL_LUT_COMPONENT_SCALE)+ v;
+
+                        if (m_lut[bin]==0 || m_lut[bin]>sig+1)
+                            m_lut[bin] = sig+1;
+                    }
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+
+void ColorLUT::clearLUT(uint8_t signum)
+{
+    int i;
+
+    for (i=0; i<CL_LUT_SIZE; i++)
+    {
+        if (signum==0)
+            m_lut[i] = 0;
+        else if (m_lut[i]==signum)
+            m_lut[i] = 0;
+    }
+}
+
+
+bool ColorLUT::growRegion(RectA *region, const Frame8 &frame, uint8_t dir)
+{
+    if (dir==0) // grow left
+    {
+        if (region->m_xOffset>=CL_GROW_INC)
+        {
+            region->m_xOffset -= CL_GROW_INC;
+            region->m_width += CL_GROW_INC;
+        }
+        else
+            return true;
+    }
+    else if (dir==1) // grow top
+    {
+        if (region->m_yOffset>=CL_GROW_INC)
+        {
+            region->m_yOffset -= CL_GROW_INC;
+            region->m_height += CL_GROW_INC;
+        }
+        else
+            return true;
+    }
+    else if (dir==2) // grow right
+    {
+        if (region->m_xOffset+region->m_width+CL_GROW_INC>frame.m_width)
+            return true;
+        region->m_width += CL_GROW_INC;
+    }
+    else if (dir==3) // grow bottom
+    {
+        if (region->m_yOffset+region->m_height+CL_GROW_INC>frame.m_height)
+            return true;
+        region->m_height += CL_GROW_INC;
+    }
+    return false;
+}
+
+
+float ColorLUT::testRegion(const RectA &region, const Frame8 &frame, UVPixel *mean, Points *points)
+{
+    UVPixel subMean;
+    float distance;
+    RectA subRegion(0, 0, CL_GROW_INC, CL_GROW_INC);
+    subRegion.m_xOffset = region.m_xOffset;
+    subRegion.m_yOffset = region.m_yOffset;
+    bool horiz = region.m_width>region.m_height;
+    uint32_t i, test, endpoint = horiz ? region.m_width : region.m_height;
+
+    for (i=0, test=0; i<endpoint; i+=CL_GROW_INC)
+    {
+        getMean(subRegion, frame, &subMean);
+        distance = sqrt((float)((mean->m_u-subMean.m_u)*(mean->m_u-subMean.m_u) + (mean->m_v-subMean.m_v)*(mean->m_v-subMean.m_v)));
+        if ((uint32_t)distance<m_maxDist)
+        {
+            int32_t n = points->size();
+            mean->m_u = ((longlong)mean->m_u*n + subMean.m_u)/(n+1);
+            mean->m_v = ((longlong)mean->m_v*n + subMean.m_v)/(n+1);
+            if (points->push_back(Point16(subRegion.m_xOffset, subRegion.m_yOffset))<0)
+                break;
+            //DBG("add %d %d %d", subRegion.m_xOffset, subRegion.m_yOffset, points->size());
+            test++;
+        }
+
+        if (horiz)
+            subRegion.m_xOffset += CL_GROW_INC;
+        else
+            subRegion.m_yOffset += CL_GROW_INC;
+    }
+
+    //DBG("return %f", (float)test*CL_GROW_INC/endpoint);
+    return (float)test*CL_GROW_INC/endpoint;
+}
+
+
+void ColorLUT::growRegion(const Frame8 &frame, const Point16 &seed, Points *points)
+{
+    uint8_t dir, done;
+    RectA region, newRegion;
+    UVPixel mean;
+    float ratio;
+
+    done = 0;
+
+    // create seed 2*CL_GROW_INCx2*CL_GROW_INC region from seed position, make sure it's within the frame
+    region.m_xOffset = seed.m_x;
+    region.m_yOffset = seed.m_y;
+    if (growRegion(&region, frame, 0))
+        done |= 1<<0;
+    else
+        points->push_back(Point16(region.m_xOffset, region.m_yOffset));
+    if (growRegion(&region, frame, 1))
+        done |= 1<<1;
+    else
+        points->push_back(Point16(region.m_xOffset, region.m_yOffset));
+    if (growRegion(&region, frame, 2))
+        done |= 1<<2;
+    else
+        points->push_back(Point16(seed.m_x, region.m_yOffset));
+    if (growRegion(&region, frame, 3))
+        done |= 1<<3;
+    else
+        points->push_back(seed);
+
+    getMean(region, frame, &mean);
+
+    while(done!=0x0f)
+    {
+        for (dir=0; dir<4; dir++)
+        {
+            newRegion = region;
+            if (done&(1<<dir))
+                continue;
+            else if (dir==0) // left
+                newRegion.m_width = 0;
+            else if (dir==1) // top
+                newRegion.m_height = 0; // top and bottom
+            else if (dir==2) // right
+            {
+                newRegion.m_xOffset += newRegion.m_width;
+                newRegion.m_width = 0;
+            }
+            else if (dir==3) // bottom
+            {
+                newRegion.m_yOffset += newRegion.m_height;
+                newRegion.m_height = 0;
+            }
+
+            if (growRegion(&newRegion, frame, dir))
+                done |= 1<<dir;
+            else
+            {
+                ratio = testRegion(newRegion, frame, &mean, points);
+                if (ratio<m_minRatio)
+                    done |= 1<<dir;
+                else
+                    growRegion(&region, frame, dir);
+            }
+        }
+    }
+}
+
+
+void ColorLUT::getMean(const RectA &region ,const Frame8 &frame, UVPixel *mean)
+{
+    UVPixel uv;
+    uint32_t n=0;
+    IterPixel ip(frame, region);
+
+    longlong usum=0, vsum=0;
+
+    while(ip.next(&uv))
+    {
+        usum += uv.m_u;
+        vsum += uv.m_v;
+        n++;
+    }
+
+    mean->m_u = usum/n;
+    mean->m_v = vsum/n;
+}
+
+void ColorLUT::setSigRange(uint8_t signum, float range)
+{
+	if (signum<1 || signum>CL_NUM_SIGNATURES)
+		return;
+	m_sigRanges[signum-1] = range;
+}
+
+void ColorLUT::setGrowDist(uint32_t dist)
+{
+	m_maxDist = dist;
+}
+
+void ColorLUT::setMinBrightness(float miny)
+{
+    m_miny = 3*((1<<8)-1)*miny;
+    if (m_miny==0)
+        m_miny = 1;
+
+}
+
+void ColorLUT::setCCGain(float gain)
+{
+    m_ccGain = gain;
+}
+
+uint32_t ColorLUT::getType(uint8_t signum)
+{
+    if (signum<1 || signum>CL_NUM_SIGNATURES)
+        return 0;
+
+    return m_signatures[signum-1].m_type;
+}
+
+#if 0
+uint32_t ColorLUT::getColor(uint8_t signum)
+{
+    int32_t r, g, b, max, u, v;
+
+    if (signum<1 || signum>CL_NUM_SIGNATURES)
+        return 0;
+
+    u = m_signatures[signum-1].m_uMean;
+    v = m_signatures[signum-1].m_vMean;
+
+    // u = r-g
+    // v = b-g
+    if (abs(u)>abs(v))
+    {
+        if (u>0)
+        {
+            r = u;
+            if (v>0)
+                g = 0;
+            else
+                g = -v;
+            b = v+g;
+        }
+        else
+        {
+            g = -u;
+            r = 0;
+            b = v+g;
+        }
+    }
+    else
+    {
+        if (v>0)
+        {
+            b = v;
+            if (u>0)
+                g = 0;
+            else
+                g = -u;
+            r = u+g;
+        }
+        else
+        {
+            g = -v;
+            b = 0;
+            r = u+g;
+        }
+    }
+
+    if (r>g)
+        max = r;
+    else
+        max = g;
+    if (b>max)
+        max = b;
+
+    // normalize
+    if (max>0)
+    {
+        r = (float)r/max*255;
+        g = (float)g/max*255;
+        b = (float)b/max*255;
+        return (r<<16) | (g<<8) | b;
+    }
+    else
+        return 0;
+}
+#endif

discovery/libs/Pixy/src/colorlut.h

@@ -0,0 +1,129 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef COLORLUT_H
+#define COLORLUT_H	  
+
+#include <inttypes.h>
+#include "simplevector.h"
+#include "pixytypes.h"
+
+#define CL_NUM_SIGNATURES               7
+#define CL_LUT_COMPONENT_SCALE          6
+#define CL_LUT_SIZE                     (1<<(CL_LUT_COMPONENT_SCALE*2))
+#define CL_LUT_ENTRY_SCALE              15
+#define CL_GROW_INC                     4
+#define CL_MIN_Y_F                      0.05 // for when generating signatures, etc
+#define CL_MIN_Y                        (int32_t)(3*((1<<8)-1)*CL_MIN_Y_F)
+#define CL_MIN_RATIO                    0.25f
+#define CL_DEFAULT_MINY                 0.1f
+#define CL_DEFAULT_SIG_RANGE			2.5f
+#define CL_MAX_DIST                     2000
+#define CL_DEFAULT_TOL                  0.9f
+#define CL_DEFAULT_CCGAIN               1.5f
+#define CL_MODEL_TYPE_COLORCODE         1
+
+
+struct ColorSignature
+{
+	ColorSignature()
+	{
+		m_uMin = m_uMax = m_uMean = m_vMin = m_vMax = m_vMean = m_type = 0;
+	}	
+
+    int32_t m_uMin;
+    int32_t m_uMax;
+    int32_t m_uMean;
+    int32_t m_vMin;
+    int32_t m_vMax;
+    int32_t m_vMean;
+	uint32_t m_rgb;
+	uint32_t m_type;
+};
+
+struct RuntimeSignature
+{
+    int32_t m_uMin;
+    int32_t m_uMax;
+    int32_t m_vMin;
+    int32_t m_vMax;
+    uint32_t m_rgbSat;
+};
+
+typedef SimpleVector<Point16> Points;
+
+class IterPixel
+{
+public:
+    IterPixel(const Frame8 &frame, const RectA &region);
+    IterPixel(const Frame8 &frame, const Points *points);
+    bool next(UVPixel *uv, RGBPixel *rgb=NULL);
+    bool reset(bool cleari=true);
+	uint32_t averageRgb(uint32_t *pixels=NULL);
+
+private:
+    bool nextHelper(UVPixel *uv, RGBPixel *rgb);
+
+    Frame8 m_frame;
+    RectA m_region;
+    uint32_t m_x, m_y;
+    uint8_t *m_pixels;
+    const Points *m_points;
+    int m_i;
+};
+
+class ColorLUT
+{
+public:
+    ColorLUT(uint8_t *lut);
+    ~ColorLUT();
+
+    int generateSignature(const Frame8 &frame, const RectA &region, uint8_t signum);
+    int generateSignature(const Frame8 &frame, const Point16 &point, Points *points, uint8_t signum);
+	ColorSignature *getSignature(uint8_t signum);
+	int setSignature(uint8_t signum, const ColorSignature &sig);
+
+    int generateLUT();
+    void clearLUT(uint8_t signum=0);
+	void updateSignature(uint8_t signum);
+    void growRegion(const Frame8 &frame, const Point16 &seed, Points *points);
+
+	void setSigRange(uint8_t signum, float range);
+	void setMinBrightness(float miny);
+	void setGrowDist(uint32_t dist);
+    void setCCGain(float gain);
+    uint32_t getType(uint8_t signum);
+
+    // these should be in little access methods, but they're here to speed things up a tad
+    ColorSignature m_signatures[CL_NUM_SIGNATURES];
+    RuntimeSignature m_runtimeSigs[CL_NUM_SIGNATURES];
+    uint32_t m_miny;
+
+private:
+    bool growRegion(RectA *region, const Frame8 &frame, uint8_t dir);
+    float testRegion(const RectA &region, const Frame8 &frame, UVPixel *mean, Points *points);
+
+    void calcRatios(IterPixel *ip, ColorSignature *sig, float ratios[]);
+    void iterate(IterPixel *ip, ColorSignature *sig);
+    void getMean(const RectA &region ,const Frame8 &frame, UVPixel *mean);
+
+    uint8_t *m_lut;
+    uint32_t m_maxDist;
+    float m_ratio;
+    float m_minRatio;
+    float m_ccGain;
+    float m_sigRanges[CL_NUM_SIGNATURES];
+};
+
+#endif // COLORLUT_H

discovery/libs/Pixy/src/debug.h

@@ -0,0 +1,7 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#define DBG(...)
+
+#endif // DEBUG_H
+

discovery/libs/Pixy/src/debuglog.h

@@ -0,0 +1,36 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef __DEBUG_H__
+#define __DEBUG_H__
+
+#include <stdarg.h>
+#include <stdio.h>
+
+static void log(const char *format, ...)
+{
+#ifdef DEBUG
+  va_list elements;
+
+  // Send debug message to stdout //
+  va_start(elements, format);
+  vfprintf(stderr,format, elements);
+  fflush(stderr);
+  va_end(elements);
+ #else
+    (void)format;
+#endif 
+}
+
+#endif

discovery/libs/Pixy/src/interpreter.hpp

@@ -0,0 +1,26 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef __INTERPRETER_HPP__
+#define __INTERPRETER_HPP__
+
+class Interpreter
+{
+  public:
+
+    virtual void interpret_data(const void *data []) = 0;
+};
+
+#endif

discovery/libs/Pixy/src/link.h

@@ -0,0 +1,76 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef LINK_H
+#define LINK_H
+
+#include <stdint.h>
+
+// flags
+#define LINK_FLAG_SHARED_MEM                            0x01
+#define	LINK_FLAG_ERROR_CORRECTED                       0x02
+
+// result codes
+#define LINK_RESULT_OK                                  0
+#define LINK_RESULT_ERROR                               -100
+#define LINK_RESULT_ERROR_RECV_TIMEOUT                  -101
+#define LINK_RESULT_ERROR_SEND_TIMEOUT                  -102
+
+// link flag index
+#define LINK_FLAG_INDEX_FLAGS                           0x00
+#define LINK_FLAG_INDEX_SHARED_MEMORY_LOCATION          0x01
+#define LINK_FLAG_INDEX_SHARED_MEMORY_SIZE              0x02
+
+
+class Link
+{
+public:
+    Link()
+    {
+        m_flags = 0;
+        m_blockSize = 0;
+    }
+    ~Link()
+    {
+    }
+
+    // the timeoutMs is a timeout value in milliseconds.  The timeout timer should expire
+    // when the data channel has been continuously idle for the specified amount of time
+    // not the summation of the idle times.
+    virtual int send(const uint8_t *data, uint32_t len, uint16_t timeoutMs) = 0;
+    virtual int receive(uint8_t *data, uint32_t len, uint16_t timeoutMs) = 0;
+    virtual void setTimer() = 0;
+    virtual uint32_t getTimer() = 0; // returns elapsed time in milliseconds since setTimer() was called
+    virtual uint32_t getFlags(uint8_t index=LINK_FLAG_INDEX_FLAGS)
+    {
+        if (index==LINK_FLAG_INDEX_FLAGS)
+            return m_flags;
+        else
+            return 0;
+    }
+    virtual uint32_t blockSize()
+    {
+        return m_blockSize;
+    }
+    virtual int getBuffer(uint8_t **, uint32_t *)
+    {
+        return LINK_RESULT_ERROR;
+    }
+
+protected:
+    uint32_t m_flags;
+    uint32_t m_blockSize;
+};
+
+#endif // LINK_H

discovery/libs/Pixy/src/pixy.cpp

@@ -0,0 +1,466 @@
+#include <stdio.h>
+#include <string.h>
+#include "pixy.h"
+#include "pixydefs.h"
+#include "pixyinterpreter.hpp"
+
+PixyInterpreter interpreter;
+
+/** 
+
+  \mainpage libpixyusb-0.4 API Reference
+
+  \section introduction Introduction
+
+  libpixyusb is an open source library that allows you to communicate with
+  Pixy over the USB protocol.
+
+  This documentation is aimed at application developers wishing to send
+  commands to Pixy or read sensor data from Pixy.
+
+  \section library_features Library features
+
+  - Read blocks with or without color codes
+  - RGB LED control (color/intensity)
+  - Auto white balance control
+  - Auto exposure compensation control
+  - Brightness control
+  - Servo position control/query
+  - Custom commands
+
+  \section dependencies Dependencies
+
+  Required to build:
+
+  - <a href=http://www.cmake.org>cmake</a>
+ 
+  Required for runtime:
+
+  - <a href=http://www.libusb.org>libusb</a>
+  - <a href=http://www.boost.org>libboost</a>
+
+  \section getting_started Getting Started
+ 
+  The libpixyusb API reference documentation can be found here:
+
+  libpixyusb API Reference
+
+  Some tutorials that use libpixyusb can be found here:
+
+  <a href=http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Raspberry_Pi>Hooking up Pixy to a Raspberry Pi</a>
+
+  <a href=http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Beaglebone_Black>Hooking up Pixy to a BeagleBone Black</a>
+
+  \section getting_help Getting Help
+
+  Tutorials, walkthroughs, and more are available on the Pixy wiki page:
+
+  <a href=http://www.cmucam.org/projects/cmucam5/wiki>Pixy Developer Wiki Page</a>
+
+  Our friendly developers and users might be able to answer your question on the forums:
+
+  <a href=http://www.cmucam.org/projects/cmucam5/boards/9>Pixy Software Discussion Forum</a>
+  
+  <a href=http://www.cmucam.org/projects/cmucam5/boards/8>Pixy Hardware Discussion Forum</a>
+
+*/
+
+// Pixy C API //
+
+extern "C" 
+{
+  static struct
+  {
+    int           error;
+    const char *  text;
+  } PIXY_ERROR_TABLE[] = {
+    { 0,                          "Success" },
+    //{ PIXY_ERROR_USB_IO,          "USB Error: I/O" },
+    //{ PIXY_ERROR_USB_BUSY,        "USB Error: Busy" },
+    //{ PIXY_ERROR_USB_NO_DEVICE,   "USB Error: No device" },
+    //{ PIXY_ERROR_USB_NOT_FOUND,   "USB Error: Target not found" },
+    { PIXY_ERROR_CHIRP,           "Chirp Protocol Error" },
+    { PIXY_ERROR_INVALID_COMMAND, "Pixy Error: Invalid command" },
+    { 0,                          0 }
+  };
+
+  static int pixy_initialized = false;
+
+  int pixy_init()
+  {
+    int return_value;
+
+    return_value = interpreter.init();
+
+    if(return_value == 0) 
+    {
+      pixy_initialized = true;
+    }
+
+    return return_value;
+  }
+
+  int pixy_get_blocks(uint16_t max_blocks, struct Block * blocks)
+  {
+    return interpreter.get_blocks(max_blocks, blocks);
+  }
+
+  int pixy_blocks_are_new()
+  {
+    return interpreter.blocks_are_new();
+  }
+
+  int pixy_service()
+  {
+      //Service calls are commented at the moment because they cause problems with USB on the discovery.
+      //TODO: Fix problems.
+       return -1;
+      //return interpreter.service();
+  }
+
+  int pixy_command(const char *name, ...)
+  {
+    va_list arguments;
+    int     return_value;
+
+    if(!pixy_initialized) return -1;
+
+    va_start(arguments, name);
+    return_value = interpreter.send_command(name, arguments);
+    va_end(arguments);
+
+    return return_value;
+  }
+
+  void pixy_close()
+  {
+    if(!pixy_initialized) return;
+
+    interpreter.close();
+  }
+
+  void pixy_error(int error_code)
+  {
+    int index;
+
+    // Convert pixy error code to string and display to stdout //
+
+    index = 0;
+
+    while(PIXY_ERROR_TABLE[index].text != 0) {
+
+      if(PIXY_ERROR_TABLE[index].error == error_code) {
+        fprintf(stderr,"%s\n", PIXY_ERROR_TABLE[index].text);
+        return;
+      }
+
+      index += 1;
+    }
+
+    fprintf(stderr,"Undefined error: [%d]\n", error_code);
+  }
+
+  int pixy_led_set_RGB(uint8_t red, uint8_t green, uint8_t blue)
+  {
+    int      chirp_response;
+    int      return_value;
+    uint32_t RGB;
+
+    // Pack the RGB value //
+    RGB = blue + (green << 8) + (red << 16);
+
+    return_value = pixy_command("led_set", INT32(RGB), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_led_set_max_current(uint32_t current)
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("led_setMaxCurrent", INT32(current), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_led_get_max_current()
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("led_getMaxCurrent", END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_set_auto_white_balance(uint8_t enable)
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("cam_setAWB", UINT8(enable), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_get_auto_white_balance()
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("cam_getAWB", END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  uint32_t pixy_cam_get_white_balance_value()
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("cam_getWBV", END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_set_white_balance_value(uint8_t red, uint8_t green, uint8_t blue)
+  {
+    int      return_value;
+    uint32_t chirp_response;
+    uint32_t white_balance;
+
+    white_balance = green + (red << 8) + (blue << 16);
+
+    return_value = pixy_command("cam_setAWB", UINT32(white_balance), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_set_auto_exposure_compensation(uint8_t enable)
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("cam_setAEC", UINT8(enable), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+}
+  
+  int pixy_cam_get_auto_exposure_compensation()
+  {
+    int      return_value;
+    uint32_t chirp_response;
+
+    return_value = pixy_command("cam_getAEC", END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_set_exposure_compensation(uint8_t gain, uint16_t compensation)
+  {
+    int      return_value;
+    uint32_t chirp_response;
+    uint32_t exposure;
+
+    exposure = gain + (compensation << 8);
+
+    return_value = pixy_command("cam_setECV", UINT32(exposure), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_get_exposure_compensation(uint8_t * gain, uint16_t * compensation)
+  {
+    uint32_t exposure;
+    int      return_value;
+
+    return_value = pixy_command("cam_getECV", END_OUT_ARGS, &exposure, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Chirp error //
+      return return_value;
+    }
+
+    if(gain == 0 || compensation == 0) {
+      // Error: Null pointer //
+      return PIXY_ERROR_INVALID_PARAMETER;
+    }
+
+    fprintf(stderr,"exp:%08x\n", exposure);
+
+    *gain         = exposure & 0xFF;
+    *compensation = 0xFFFF & (exposure >> 8);
+
+    return 0;
+  }
+
+  int pixy_cam_set_brightness(uint8_t brightness)
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("cam_setBrightness", UINT8(brightness), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_cam_get_brightness()
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("cam_getBrightness", END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_rcs_get_position(uint8_t channel)
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("rcs_getPos", UINT8(channel), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_rcs_set_position(uint8_t channel, uint16_t position)
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("rcs_setPos", UINT8(channel), INT16(position), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_rcs_set_frequency(uint16_t frequency)
+  {
+    int chirp_response;
+    int return_value;
+
+    return_value = pixy_command("rcs_setFreq", UINT16(frequency), END_OUT_ARGS, &chirp_response, END_IN_ARGS);
+
+   if (return_value < 0) {
+      // Error //
+      return return_value;
+    } else {
+      // Success //
+      return chirp_response;
+    }
+  }
+
+  int pixy_get_firmware_version(uint16_t * major, uint16_t * minor, uint16_t * build)
+  {
+    uint16_t * pixy_version;
+    uint32_t   version_length;
+    uint32_t   response;
+    uint16_t   version[3];
+    int        return_value;
+
+    if(major == 0 || minor == 0 || build == 0) {
+      // Error: Null pointer //
+      return PIXY_ERROR_INVALID_PARAMETER;
+    }
+
+    return_value = pixy_command("version",  END_OUT_ARGS, &response, &version_length, &pixy_version, END_IN_ARGS);
+
+    if (return_value < 0) {
+      // Error //
+      return return_value;
+    }
+
+    memcpy((void *) version, pixy_version, 3 * sizeof(uint16_t));
+
+    *major = version[0];
+    *minor = version[1];
+    *build = version[2];
+
+    return 0;
+  }
+}

discovery/libs/Pixy/src/pixyinterpreter.cpp

@@ -0,0 +1,298 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include <string.h>
+#include <stdio.h>
+#include "pixyinterpreter.hpp"
+
+PixyInterpreter::PixyInterpreter()
+{
+  init_ = false;
+  receiver_    = NULL;
+}
+
+PixyInterpreter::~PixyInterpreter()
+{
+  close();
+}
+
+int PixyInterpreter::init()
+{
+  int USB_return_value;
+
+  if(init_ == true)
+  {
+    fprintf(stderr, "libpixy: Already initialized.");
+    return 0;
+  }
+
+  USB_return_value = link_.open();
+
+  if(USB_return_value < 0) {
+    return USB_return_value;
+  }
+
+  receiver_ = new ChirpReceiver(&link_, this);
+
+  init_ = true;
+
+  return 0;
+}
+
+void PixyInterpreter::close()
+{  
+  if (receiver_)  
+  {
+    delete receiver_;
+    receiver_ = NULL;
+  }
+  init_ = false;
+}
+
+int PixyInterpreter::get_blocks(int max_blocks, Block * blocks)
+{
+  uint16_t number_of_blocks_to_copy;
+  uint16_t index;
+
+  // Check parameters //
+
+  if(max_blocks < 0 || blocks == 0) {
+    return PIXY_ERROR_INVALID_PARAMETER;
+  }
+
+  number_of_blocks_to_copy = (max_blocks >= blocks_.size() ? blocks_.size() : max_blocks);
+  
+  // Copy blocks //
+  
+  for (index = 0; index != number_of_blocks_to_copy; ++index) {
+    memcpy(&blocks[index], &blocks_[index], sizeof(Block));
+  }
+
+  blocks_are_new_ = false;
+
+  return number_of_blocks_to_copy;
+}
+
+int PixyInterpreter::send_command(const char * name, ...)
+{
+  va_list arguments;
+  int     return_value;
+
+  va_start(arguments, name);
+  return_value = send_command(name, arguments);
+  va_end(arguments);
+
+  return return_value;
+}
+
+int PixyInterpreter::send_command(const char * name, va_list args)
+{
+  ChirpProc procedure_id;
+  int       return_value;
+  va_list   arguments;
+
+  va_copy(arguments, args);
+
+  // Request chirp procedure id for 'name'. //
+  procedure_id = receiver_->getProc(name);
+
+  // Was there an error requesting procedure id? //
+  if (procedure_id < 0) {
+    // Request error //
+    va_end(arguments);
+    return PIXY_ERROR_INVALID_COMMAND;
+  }
+
+  // Execute chirp synchronous remote procedure call //
+  return_value = receiver_->call(SYNC, procedure_id, arguments); 
+  va_end(arguments);
+
+  return return_value;
+}
+
+int PixyInterpreter::service()
+{
+    if(!init_) return -1;
+    receiver_->service(false);
+    return 0; //success
+}
+
+
+void PixyInterpreter::interpret_data(const void * chirp_data[])
+{
+  uint8_t  chirp_message;
+  uint32_t chirp_type;
+  if (chirp_data[0]) {
+
+    chirp_message = Chirp::getType(chirp_data[0]);
+
+    switch(chirp_message) {
+      
+      case CRP_TYPE_HINT:
+        
+        chirp_type = * static_cast<const uint32_t *>(chirp_data[0]);
+
+        switch(chirp_type) {
+
+          case FOURCC('B', 'A', '8', '1'):
+            break;
+          case FOURCC('C', 'C', 'Q', '1'):
+            break;
+          case FOURCC('C', 'C', 'B', '1'):
+            interpret_CCB1(chirp_data + 1);
+            break;
+          case FOURCC('C', 'C', 'B', '2'):
+            interpret_CCB2(chirp_data + 1);
+            break;
+          case FOURCC('C', 'M', 'V', '1'):
+            break;
+          default:
+            fprintf(stderr,"libpixy: Chirp hint [%u] not recognized.\n", chirp_type);
+            break;
+        }
+
+        break;
+
+      case CRP_HSTRING:
+
+        break;
+      
+      default:
+       
+       fprintf(stderr, "libpixy: Unknown message received from Pixy: [%u]\n", chirp_message);
+       break;
+    }
+  } 
+}
+
+void PixyInterpreter::interpret_CCB1(const void * CCB1_data[])
+{
+  uint32_t       number_of_blobs;
+  const BlobA *  blobs;
+  
+  // Add blocks with normal signatures //
+  
+  number_of_blobs = * static_cast<const uint32_t *>(CCB1_data[3]);
+  blobs           = static_cast<const BlobA *>(CCB1_data[4]);
+  
+  number_of_blobs /= sizeof(BlobA) / sizeof(uint16_t);
+
+  add_normal_blocks(blobs, number_of_blobs);
+  blocks_are_new_ = true;
+}
+
+
+void PixyInterpreter::interpret_CCB2(const void * CCB2_data[])
+{
+  uint32_t       number_of_blobs;
+  const BlobA *  A_blobs;
+  const BlobB *  B_blobs;
+
+  // The blocks container will only contain the newest //
+  // blocks                                            //
+  blocks_.clear();
+
+  // Add blocks with color code signatures //
+
+  number_of_blobs = * static_cast<const uint32_t *>(CCB2_data[5]);
+  B_blobs         = static_cast<const BlobB *>(CCB2_data[6]);
+  
+  number_of_blobs /= sizeof(BlobB) / sizeof(uint16_t);
+  add_color_code_blocks(B_blobs, number_of_blobs);
+
+  // Add blocks with normal signatures //
+
+  number_of_blobs = * static_cast<const uint32_t *>(CCB2_data[3]);
+  A_blobs         = static_cast<const BlobA *>(CCB2_data[4]);
+  
+  number_of_blobs /= sizeof(BlobA) / sizeof(uint16_t);
+  
+  add_normal_blocks(A_blobs, number_of_blobs);
+  blocks_are_new_ = true;
+}
+
+void PixyInterpreter::add_normal_blocks(const BlobA * blocks, uint32_t count)
+{
+  uint32_t index;
+  Block    block;
+
+  for (index = 0; index != count; ++index) {
+
+    // Decode CCB1 'Normal' Signature Type //
+
+    block.type      = PIXY_BLOCKTYPE_NORMAL;
+    block.signature = blocks[index].m_model;
+    block.width     = blocks[index].m_right - blocks[index].m_left;
+    block.height    = blocks[index].m_bottom - blocks[index].m_top;
+    block.x         = blocks[index].m_left + block.width / 2;
+    block.y         = blocks[index].m_top + block.height / 2;
+
+    // Angle is not a valid parameter for 'Normal'  //
+    // signature types. Setting to zero by default. //
+    block.angle     = 0;
+      
+    // Store new block in block buffer //
+
+    if (blocks_.size() == PIXY_BLOCK_CAPACITY) {
+      // Blocks buffer is full - replace oldest received block with newest block //
+      blocks_.erase(blocks_.begin());
+      blocks_.push_back(block);
+    } else {
+      // Add new block to blocks buffer //
+      blocks_.push_back(block);
+    }
+  }
+}
+
+void PixyInterpreter::add_color_code_blocks(const BlobB * blocks, uint32_t count)
+{
+  uint32_t index;
+  Block    block;
+    
+  for (index = 0; index != count; ++index) {
+
+    // Decode 'Color Code' Signature Type //
+
+    block.type      = PIXY_BLOCKTYPE_COLOR_CODE;
+    block.signature = blocks[index].m_model;
+    block.width     = blocks[index].m_right - blocks[index].m_left;
+    block.height    = blocks[index].m_bottom - blocks[index].m_top;
+    block.x         = blocks[index].m_left + block.width / 2;
+    block.y         = blocks[index].m_top + block.height / 2;
+    block.angle     = blocks[index].m_angle;
+
+    // Store new block in block buffer //
+
+    if (blocks_.size() == PIXY_BLOCK_CAPACITY) {
+      // Blocks buffer is full - replace oldest received block with newest block //
+      blocks_.erase(blocks_.begin());
+      blocks_.push_back(block);
+    } else {
+      // Add new block to blocks buffer //
+      blocks_.push_back(block);
+    }
+  }
+}
+
+int PixyInterpreter::blocks_are_new()
+{
+  if (blocks_are_new_) {
+    // Fresh blocks!! :D //
+    return 1;
+  } else {
+    // Stale blocks... :\ //
+    return 0;
+  }
+}

discovery/libs/Pixy/src/pixyinterpreter.hpp

@@ -0,0 +1,149 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef __PIXYINTERPRETER_HPP__
+#define __PIXYINTERPRETER_HPP__
+
+#include <vector>
+#include "pixytypes.h"
+#include "pixy.h"
+#include "pixydefs.h"
+#include "usblink.h"
+#include "interpreter.hpp"
+#include "chirpreceiver.hpp"
+
+#define PIXY_BLOCK_CAPACITY         250
+
+class PixyInterpreter : public Interpreter
+{
+  public:
+
+    PixyInterpreter();
+    ~PixyInterpreter();
+
+    /**
+      @brief  Spawns an 'interpreter' thread which attempts to 
+              connect to Pixy using the USB interface. 
+              On successful connection, this thread will 
+              capture and store Pixy 'block' object data 
+              which can be retreived using the getBlocks()
+              method.
+       @return   0    Success
+       @return  -1    Error: Unable to open pixy USB device
+
+    */
+  
+    int init();
+    
+    /**
+      @brief  Terminates the USB connection to Pixy and
+              the 'iterpreter' thread.
+    */
+    void close();
+
+   /**
+     @brief      Get status of the block data received from Pixy. 
+ 
+     @return  0  Stale Data: Block data has previously been retrieved using 'pixy_get_blocks()'.
+     @return  1  New Data: Pixy sent new data that has not been retrieve yet.
+   */
+    int blocks_are_new();
+
+    /**
+      @brief      Copies up to 'max_blocks' number of Blocks to the address pointed
+                  to by 'blocks'. 
+      @param[in]  max_blocks Maximum number of Blocks to copy to the address pointed to
+                             by 'blocks'.
+      @param[out] blocks     Address of an array in which to copy the blocks to.
+                             The array must be large enough to write 'max_blocks' number
+                             of Blocks to.
+      @return  Non-negative                  Success: Number of blocks copied
+      @return  PIXY_ERROR_USB_IO             USB Error: I/O
+      @return  PIXY_ERROR_NOT_FOUND          USB Error: Pixy not found
+      @return  PIXY_ERROR_USB_BUSY           USB Error: Busy
+      @return  PIXY_ERROR_USB_NO_DEVICE      USB Error: No device
+      @return  PIXY_ERROR_INVALID_PARAMETER  Invalid pararmeter specified
+    */
+    int get_blocks(int max_blocks, Block * blocks);
+
+    /**
+      @brief         Sends a command to Pixy.
+      @param[in]     name       Remote procedure call identifier string.
+      @param[in,out] arguments  Argument list to function call.
+      @return        -1         Error
+    */
+    int send_command(const char * name, va_list arguments);
+
+    /**
+      @brief         Sends a command to Pixy.
+      @param[in]     name       Remote procedure call identifier string.
+      @return        -1         Error
+    */
+    int send_command(const char * name, ...);
+
+
+
+    int service();
+
+  private:
+    
+    ChirpReceiver *    receiver_;
+    USBLink            link_;
+    std::vector<Block> blocks_;
+    bool               blocks_are_new_;
+    bool               init_;
+
+
+    /**
+      @brief Interprets data sent from Pixy over the Chirp protocol.
+
+      @param[in] data  Incoming Chirp protocol data from Pixy.
+    */
+    void interpret_data(const void * chrip_data[]);
+
+    /**
+      @brief Interprets CCB1 messages sent from Pixy.
+
+      @param[in] data  Incoming Chirp protocol data from Pixy.
+    */
+    void interpret_CCB1(const void * data[]);
+
+    /**
+      @brief Interprets CCB2 messages sent from Pixy.
+
+      @param[in] data  Incoming Chirp protocol data from Pixy.
+    */
+    void interpret_CCB2(const void * data[]);
+
+    /**
+      @brief Adds blocks with normal signatures to the PixyInterpreter
+             'blocks_' buffer.
+
+      @param[in] blocks  An array of normal signature blocks to add to buffer.
+      @param[in] count   Size of the 'blocks' array.
+    */
+    void add_normal_blocks(const BlobA * blocks, uint32_t count);
+
+    /**
+      @brief Adds blocks with color code signatures to the PixyInterpreter
+             'blocks_' buffer.
+
+      @param[in] blocks  An array of color code signature blocks to add to buffer.
+      @param[in] count   Size of the 'blocks' array.
+    */
+    void add_color_code_blocks(const BlobB * blocks, uint32_t count);
+};
+
+#endif

discovery/libs/Pixy/src/pixytypes.h

@@ -0,0 +1,271 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef PIXYTYPES_H
+#define PIXYTYPES_H
+
+#include <stdint.h>
+
+#define RENDER_FLAG_FLUSH            0x01	// add to stack, render immediately
+#define RENDER_FLAG_BLEND            0x02	// blend with a previous images in image stack
+
+#define PRM_FLAG_INTERNAL            0x00000001
+#define PRM_FLAG_ADVANCED            0x00000002
+#define PRM_FLAG_HEX_FORMAT          0x00000010
+#define PRM_FLAG_SIGNED              0x00000080
+
+// render-specific flags
+#define PRM_FLAG_SLIDER              0x00000100
+#define PRM_FLAG_CHECKBOX            0x00000200
+#define PRM_FLAG_PATH                0x00000400
+
+// events
+#define EVT_PARAM_CHANGE             1
+
+struct Point16
+{
+    Point16()
+    {
+        m_x = m_y = 0;
+    }
+
+    Point16(int16_t x, int16_t y)
+    {
+        m_x = x;
+        m_y = y;
+    }
+
+    int16_t m_x;
+    int16_t m_y;
+};
+
+struct Point32
+{
+    Point32()
+    {
+        m_x = m_y = 0;
+    }
+
+    Point32(int32_t x, int32_t y)
+    {
+        m_x = x;
+        m_y = y;
+    }
+
+    int32_t m_x;
+    int32_t m_y;
+};
+
+struct Frame8
+{
+    Frame8()
+    {
+        m_pixels = (uint8_t *)NULL;
+        m_width = m_height = 0;
+    }
+
+    Frame8(uint8_t *pixels, uint16_t width, uint16_t height)
+    {
+        m_pixels = pixels;
+        m_width = width;
+        m_height = height;
+    }
+
+    uint8_t *m_pixels;
+    int16_t m_width;
+    int16_t m_height;
+};
+
+struct RectA
+{
+    RectA()
+    {
+        m_xOffset = m_yOffset = m_width = m_height = 0;
+    }
+
+    RectA(uint16_t xOffset, uint16_t yOffset, uint16_t width, uint16_t height)
+    {
+        m_xOffset = xOffset;
+        m_yOffset = yOffset;
+        m_width = width;
+        m_height = height;
+    }
+
+    uint16_t m_xOffset;
+    uint16_t m_yOffset;
+    uint16_t m_width;
+    uint16_t m_height;
+};
+
+struct RectB
+{
+    RectB()
+    {
+        m_left = m_right = m_top = m_bottom = 0;
+    }
+
+    RectB(uint16_t left, uint16_t right, uint16_t top, uint16_t bottom)
+    {
+        m_left = left;
+        m_right = right;
+        m_top = top;
+        m_bottom = bottom;
+    }
+
+    uint16_t m_left;
+    uint16_t m_right;
+    uint16_t m_top;
+    uint16_t m_bottom;
+};
+
+struct BlobA
+{
+    BlobA()
+    {
+        m_model = m_left = m_right = m_top = m_bottom = 0;
+    }
+
+    BlobA(uint16_t model, uint16_t left, uint16_t right, uint16_t top, uint16_t bottom)
+    {
+        m_model = model;
+        m_left = left;
+        m_right = right;
+        m_top = top;
+        m_bottom = bottom;
+    }
+
+    uint16_t m_model;
+    uint16_t m_left;
+    uint16_t m_right;
+    uint16_t m_top;
+    uint16_t m_bottom;
+};
+
+struct BlobB
+{
+    BlobB()
+    {
+        m_model = m_left = m_right = m_top = m_bottom = m_angle = 0;
+    }
+
+    BlobB(uint16_t model, uint16_t left, uint16_t right, uint16_t top, uint16_t bottom, int16_t angle)
+    {
+        m_model = model;
+        m_left = left;
+        m_right = right;
+        m_top = top;
+        m_bottom = bottom;
+        m_angle = angle;
+    }
+
+    uint16_t m_model;
+    uint16_t m_left;
+    uint16_t m_right;
+    uint16_t m_top;
+    uint16_t m_bottom;
+    int16_t m_angle;
+};
+
+
+struct HuePixel
+{
+    HuePixel()
+    {
+        m_u = m_v = 0;
+    }
+
+    HuePixel(int8_t u, int8_t v)
+    {
+        m_u = u;
+        m_v = v;
+    }
+
+    int8_t m_u;
+    int8_t m_v;
+};
+
+struct Fpoint
+{
+    Fpoint()
+    {
+        m_x = m_y = 0.0;
+    }
+
+    Fpoint(float x, float y)
+    {
+        m_x = x;
+        m_y = y;
+    }
+
+    float m_x;
+    float m_y;
+};
+
+struct UVPixel
+{
+    UVPixel()
+    {
+        m_u = m_v = 0;
+    }
+
+    UVPixel(int32_t u, int32_t v)
+    {
+        m_u = u;
+        m_v = v;
+    }
+
+    int32_t m_u;
+    int32_t m_v;
+};
+
+struct RGBPixel
+{
+    RGBPixel()
+    {
+        m_r = m_g = m_b = 0;
+    }
+
+    RGBPixel(uint8_t r, uint8_t g, uint8_t b)
+    {
+        m_r = r;
+        m_g = g;
+        m_b = b;
+    }
+
+    uint8_t m_r;
+	uint8_t m_g;
+	uint8_t m_b;
+};
+
+
+struct Line
+{
+    Line()
+    {
+        m_slope = m_yi = 0.0;
+    }
+    Line(float slope, float yi)
+    {
+        m_slope = slope;
+        m_yi = yi;
+    }
+
+    float m_slope;
+    float m_yi;
+};
+
+typedef long long longlong;
+
+#endif // PIXYTYPES_H

discovery/libs/Pixy/src/qqueue.cpp.notneeded

@@ -0,0 +1,103 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#include <string.h>
+#include "qqueue.h"
+#ifdef PIXY
+#include <pixyvals.h>
+#endif
+
+Qqueue::Qqueue()
+{
+#ifdef PIXY
+    m_fields = (QqueueFields *)QQ_LOC;
+#else
+    m_fields = (QqueueFields *)(new uint8_t[QQ_SIZE]);
+#endif
+    memset((void *)m_fields, 0, sizeof(QqueueFields));
+}
+
+Qqueue::~Qqueue()
+{
+#ifdef PIXY
+#else
+    delete [] m_fields;
+#endif
+}
+
+uint32_t Qqueue::dequeue(Qval *val)
+{
+    uint16_t len = m_fields->produced - m_fields->consumed;
+    if (len)
+    {
+        *val = m_fields->data[m_fields->readIndex++];
+        m_fields->consumed++;
+        if (m_fields->readIndex==QQ_MEM_SIZE)
+            m_fields->readIndex = 0;
+        return 1;
+    }
+    return 0;
+}
+
+#ifndef PIXY
+int Qqueue::enqueue(Qval *val)
+{
+    uint16_t len = m_fields->produced - m_fields->consumed;
+    uint16_t freeLen = 	QQ_MEM_SIZE-len;
+    if (freeLen>0)
+    {
+        m_fields->data[m_fields->writeIndex++] = *val;
+        m_fields->produced++;
+        if (m_fields->writeIndex==QQ_MEM_SIZE)
+            m_fields->writeIndex = 0;
+        return 1;
+    }
+    return 0;
+
+}
+
+#endif
+
+uint32_t Qqueue::readAll(Qval *mem, uint32_t size)
+{
+    uint16_t len = m_fields->produced - m_fields->consumed;
+    uint16_t i, j;
+
+    for (i=0, j=m_fields->readIndex; i<len && i<size; i++)
+    {
+        mem[i] = m_fields->data[j++];
+        if (j==QQ_MEM_SIZE)
+            j = 0;
+    }
+    // flush the rest
+    m_fields->consumed += len;
+    m_fields->readIndex += len;
+    if (m_fields->readIndex>=QQ_MEM_SIZE)
+        m_fields->readIndex -= QQ_MEM_SIZE;
+
+    return i;
+}
+
+void Qqueue::flush()
+{
+    uint16_t len = m_fields->produced - m_fields->consumed;
+
+    m_fields->consumed += len;
+    m_fields->readIndex += len;
+    if (m_fields->readIndex>=QQ_MEM_SIZE)
+        m_fields->readIndex -= QQ_MEM_SIZE;
+}
+
+

discovery/libs/Pixy/src/qqueue.h

@@ -0,0 +1,105 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+#ifndef _QQUEUE_H
+#define _QQUEUE_H
+#include <inttypes.h>
+
+#define QQ_LOC        SRAM4_LOC
+#ifdef PIXY
+#define QQ_SIZE       0x3c00
+#else
+#define QQ_SIZE       0x30000
+#endif
+#define QQ_MEM_SIZE  ((QQ_SIZE-sizeof(struct QqueueFields)+sizeof(Qval))/sizeof(Qval))
+
+#ifdef __cplusplus  
+struct Qval
+#else
+typedef struct
+#endif
+{
+#ifdef __cplusplus  
+    Qval()
+    {
+        m_u = m_v = m_y = m_col = 0;
+    }
+
+    Qval(int16_t u, int16_t v, uint16_t y, uint16_t col)
+    {
+        m_u = u;
+        m_v = v;
+        m_y = y;
+        m_col = col;
+    }
+#endif
+
+    uint16_t m_col;
+    int16_t m_v;
+    int16_t m_u;
+    uint16_t m_y;
+
+#ifdef __cplusplus  
+}; 
+#else
+} Qval;
+#endif
+
+
+struct QqueueFields
+{
+    uint16_t readIndex;
+    uint16_t writeIndex;
+
+    uint16_t produced;
+    uint16_t consumed;
+
+    // (array size below doesn't matter-- we're just going to cast a pointer to this struct)
+    Qval data[1]; // data
+};
+
+#ifdef __cplusplus  // M4 is C++ and the "consumer" of data
+
+class Qqueue
+{
+public:
+    Qqueue();
+    ~Qqueue();
+
+    uint32_t dequeue(Qval *val);
+	uint32_t queued()
+	{
+		return m_fields->produced - m_fields->consumed;
+	}
+#ifndef PIXY
+    int enqueue(Qval *val);
+#endif
+
+    uint32_t readAll(Qval *mem, uint32_t size);
+    void flush();
+
+private:
+    QqueueFields *m_fields;
+};
+
+#else //  M0 is C and the "producer" of data (Qvals)
+
+uint32_t qq_enqueue(const Qval *val);
+uint16_t qq_free(void);
+
+extern struct QqueueFields *g_qqueue;
+
+#endif
+
+#endif

discovery/libs/Pixy/src/simplevector.h

@@ -0,0 +1,95 @@
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef SIMPLEVECTOR_H
+#define SIMPLEVECTOR_H
+
+#include <new>
+
+#define SPARE_CAPACITY   16
+
+template <typename Object> class SimpleVector
+{
+public:
+
+    SimpleVector(int initSize = 0)
+        : m_size(0), m_capacity(initSize + SPARE_CAPACITY)
+    { m_objects = new Object[m_capacity]; }
+
+    ~SimpleVector()
+    { delete [] m_objects; }
+
+    int resize(int newCapacity)
+    {
+        if(newCapacity < m_size)
+            return 0;
+
+        Object *oldArray = m_objects;
+
+        m_objects = new (std::nothrow) Object[newCapacity];
+        if (m_objects==NULL)
+        {
+            m_objects = oldArray;
+            return -1;
+        }
+        for(int k = 0; k<m_size; k++)
+            m_objects[k] = oldArray[k];
+
+        m_capacity = newCapacity;
+
+        delete [] oldArray;
+        return 0;
+    }
+
+    Object & operator[](int index)
+    { return m_objects[index]; }
+
+    const Object& operator[](int index) const
+    { return m_objects[index]; }
+
+    bool empty() const
+    { return size()==0; }
+
+    int size() const
+    { return m_size; }
+
+    int capacity() const
+    { return m_capacity; }
+
+	const Object *data()
+	{ return m_objects; }
+
+    int push_back(const Object& x)
+    {
+        if(m_size == m_capacity)
+            if (resize(m_capacity + SPARE_CAPACITY)<0)
+                return -1;
+        m_objects[m_size++] = x;
+        return 0;
+    }
+
+    void pop_back()
+    { m_size--; }
+
+	void clear()
+	{ m_size = 0; }
+
+private:
+    int m_size;
+    int m_capacity;
+    Object *m_objects;
+};
+
+#endif // SIMPLEVECTOR_H

discovery/libs/Pixy/src/usblink.cpp

@@ -0,0 +1,52 @@
+#include <unistd.h>
+#include <stdio.h>
+#include "usblink.h"
+#include "pixy.h"
+#include "debuglog.h"
+
+extern "C" {
+  extern int USBH_LL_open();
+  extern int USBH_LL_close();
+  extern int USBH_LL_send(const uint8_t *data, uint32_t len, uint16_t timeoutMs);
+  extern int USBH_LL_receive(uint8_t *data, uint32_t len, uint16_t timeoutMs);
+  extern void USBH_LL_setTimer();
+  extern uint32_t USBH_LL_getTimer();
+}
+
+
+USBLink::USBLink()
+{
+  m_blockSize = 64;
+  m_flags = LINK_FLAG_ERROR_CORRECTED;
+}
+
+USBLink::~USBLink()
+{
+    USBH_LL_close();
+}
+
+int USBLink::open()
+{
+    return USBH_LL_open();
+}
+
+int USBLink::send(const uint8_t *data, uint32_t len, uint16_t timeoutMs)
+{
+    return USBH_LL_send(data,len,timeoutMs);
+}
+
+int USBLink::receive(uint8_t *data, uint32_t len, uint16_t timeoutMs)
+{
+    return USBH_LL_receive(data,len,timeoutMs);
+}
+
+void USBLink::setTimer()
+{
+    USBH_LL_setTimer();
+}
+
+uint32_t USBLink::getTimer()
+{
+  return USBH_LL_getTimer();
+}
+

discovery/libs/Pixy/src/usblink.h

@@ -0,0 +1,20 @@
+#ifndef __USBLINK_H__
+#define __USBLINK_H__
+
+#include "link.h"
+
+class USBLink : public Link
+{
+public:
+    USBLink();
+    ~USBLink();
+
+    int open();
+    virtual int send(const uint8_t *data, uint32_t len, uint16_t timeoutMs);
+    virtual int receive(uint8_t *data, uint32_t len, uint16_t timeoutMs);
+    virtual void setTimer();
+    virtual uint32_t getTimer();
+};
+
+#endif
+

discovery/libs/StmCoreNPheriph/Makefile

@@ -21,7 +21,7 @@ ARCH_FLAGS=-mthumb -mcpu=cortex-m4 $(FP_FLAGS)
 
 #Compiler, Linker Options
 CPPFLAGS=-I$(INC_DIR)
-CFLAGS=$(ARCH_FLAGS) -O0 -g #-ffunction-sections -fdata-sections -g
+CFLAGS=$(ARCH_FLAGS) -O0 -g -ffunction-sections -fdata-sections 
 #CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 -mthumb-interwork
 #CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
 

discovery/libs/StmCoreNPheriph/inc/stm32f4xx.h

@@ -120,8 +120,8 @@
   */           
 
 #if !defined  (HSE_VALUE) 
-  #define HSE_VALUE    ((uint32_t)25000000) /*!< Value of the External oscillator in Hz */
+  //#define HSE_VALUE    ((uint32_t)25000000) /*!< Value of the External oscillator in Hz */
-  
+  #define HSE_VALUE    ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
 #endif /* HSE_VALUE */
 
 /**