diff --git a/common/app/app.c b/common/app/app.c index 45f97b8..7f00448 100644 --- a/common/app/app.c +++ b/common/app/app.c @@ -1,13 +1,15 @@ #include "app.h" #include "tft.h" #include "system.h" +#include "pixy.h" #include void app_init() { system_init(); tft_init(); - + pixy_init(); + //only testwise tft_clear(WHITE); tft_draw_pixel(0,0,GREEN); @@ -21,8 +23,23 @@ void app_init() { } + +int colorind; +const uint32_t colors [] = {0xFF0000, 0x00FF00,0x0000FF,0xFFFF00,0x00FFFF,0xFF00FF,0xFFFFFF,0x000000}; +const int num_colors = sizeof(colors)/sizeof(uint32_t); + //app event loop void app_process() { -/// printf("hello world\n"); + pixy_service(); + if(colorind==0) { + pixy_led_set_max_current(5); + } + + int32_t response; + int return_value; + return_value = pixy_command("led_set", INT32(colors[colorind++]), END_OUT_ARGS, &response, END_IN_ARGS); + colorind%=num_colors; + + system_delay(500); } diff --git a/common/lowlevel/ll_system.h b/common/lowlevel/ll_system.h index 89e18b9..3f427da 100644 --- a/common/lowlevel/ll_system.h +++ b/common/lowlevel/ll_system.h @@ -1,2 +1,5 @@ +#include bool ll_system_init(); +void ll_system_delay(uint32_t msec); + diff --git a/common/libs/Pixy/pixy.h b/common/pixy/pixy.h similarity index 96% rename from common/libs/Pixy/pixy.h rename to common/pixy/pixy.h index 5bafeaa..f723afb 100644 --- a/common/libs/Pixy/pixy.h +++ b/common/pixy/pixy.h @@ -279,14 +279,6 @@ extern "C" */ int pixy_get_firmware_version(uint16_t * major, uint16_t * minor, uint16_t * build); - 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(); - - #ifdef __cplusplus } #endif diff --git a/common/libs/Pixy/pixydefs.h b/common/pixy/pixydefs.h similarity index 100% rename from common/libs/Pixy/pixydefs.h rename to common/pixy/pixydefs.h diff --git a/common/system/system.c b/common/system/system.c index 01387d6..6cb0887 100644 --- a/common/system/system.c +++ b/common/system/system.c @@ -6,3 +6,10 @@ bool system_init() { return ll_system_init(); } + + +void system_delay(uint32_t msec) { + ll_system_delay(msec); +} + + diff --git a/common/system/system.h b/common/system/system.h index 98e9b50..9513dd2 100644 --- a/common/system/system.h +++ b/common/system/system.h @@ -1,4 +1,5 @@ #include - +#include bool system_init(); +void system_delay(uint32_t msec); diff --git a/discovery/Makefile b/discovery/Makefile index 4165b7c..60f2e88 100644 --- a/discovery/Makefile +++ b/discovery/Makefile @@ -34,8 +34,7 @@ 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) ! -path \*/libs) -INCLUDES+=$(COMMON_DIR)/libs/Pixy +INCLUDES+=$(shell find $(COMMON_DIR) -maxdepth 1 -type d ! -path $(COMMON_DIR)) INCLUDES:=$(addprefix -I,$(INCLUDES)) @@ -47,7 +46,7 @@ CFLAGS=$(ARCH_FLAGS) -O0 -g LIBS=pixy usbhost coreperiph stdc++ LIBSEARCHDIRS=$(LIB_DIR)/StmCoreNPheriph LIBSEARCHDIRS+=$(LIB_DIR)/StmUsbHost -LIBSEARCHDIRS+=$(COMMON_DIR)/libs/Pixy +LIBSEARCHDIRS+=$(LIB_DIR)/Pixy LDFLAGS=--specs=nosys.specs -Wl,--gc-sections diff --git a/common/libs/Pixy/.gitignore b/discovery/libs/Pixy/.gitignore similarity index 100% rename from common/libs/Pixy/.gitignore rename to discovery/libs/Pixy/.gitignore diff --git a/common/libs/Pixy/Makefile b/discovery/libs/Pixy/Makefile similarity index 97% rename from common/libs/Pixy/Makefile rename to discovery/libs/Pixy/Makefile index 7c9d2a6..0115f16 100644 --- a/common/libs/Pixy/Makefile +++ b/discovery/libs/Pixy/Makefile @@ -12,7 +12,7 @@ MKDIR=mkdir -p #Directories SRC_DIR=./src -INC_DIR=./ +INC_DIR=../../../common/pixy OBJ_DIR=./obj #Architecture flags diff --git a/common/libs/Pixy/hello_pixy.cpp b/discovery/libs/Pixy/hello_pixy.cpp similarity index 100% rename from common/libs/Pixy/hello_pixy.cpp rename to discovery/libs/Pixy/hello_pixy.cpp diff --git a/common/libs/Pixy/src/blob.cpp.notneeded b/discovery/libs/Pixy/src/blob.cpp.notneeded similarity index 100% rename from common/libs/Pixy/src/blob.cpp.notneeded rename to discovery/libs/Pixy/src/blob.cpp.notneeded diff --git a/common/libs/Pixy/src/blob.h b/discovery/libs/Pixy/src/blob.h similarity index 100% rename from common/libs/Pixy/src/blob.h rename to discovery/libs/Pixy/src/blob.h diff --git a/common/libs/Pixy/src/blobs.cpp.notneeded b/discovery/libs/Pixy/src/blobs.cpp.notneeded similarity index 100% rename from common/libs/Pixy/src/blobs.cpp.notneeded rename to discovery/libs/Pixy/src/blobs.cpp.notneeded diff --git a/common/libs/Pixy/src/blobs.h b/discovery/libs/Pixy/src/blobs.h similarity index 100% rename from common/libs/Pixy/src/blobs.h rename to discovery/libs/Pixy/src/blobs.h diff --git a/common/libs/Pixy/src/calc.cpp.notneeded b/discovery/libs/Pixy/src/calc.cpp.notneeded similarity index 100% rename from common/libs/Pixy/src/calc.cpp.notneeded rename to discovery/libs/Pixy/src/calc.cpp.notneeded diff --git a/common/libs/Pixy/src/calc.h b/discovery/libs/Pixy/src/calc.h similarity index 100% rename from common/libs/Pixy/src/calc.h rename to discovery/libs/Pixy/src/calc.h diff --git a/common/libs/Pixy/src/chirp.cpp b/discovery/libs/Pixy/src/chirp.cpp similarity index 100% rename from common/libs/Pixy/src/chirp.cpp rename to discovery/libs/Pixy/src/chirp.cpp diff --git a/common/libs/Pixy/src/chirp.hpp b/discovery/libs/Pixy/src/chirp.hpp similarity index 100% rename from common/libs/Pixy/src/chirp.hpp rename to discovery/libs/Pixy/src/chirp.hpp diff --git a/common/libs/Pixy/src/chirpreceiver.cpp b/discovery/libs/Pixy/src/chirpreceiver.cpp similarity index 100% rename from common/libs/Pixy/src/chirpreceiver.cpp rename to discovery/libs/Pixy/src/chirpreceiver.cpp diff --git a/common/libs/Pixy/src/chirpreceiver.hpp b/discovery/libs/Pixy/src/chirpreceiver.hpp similarity index 100% rename from common/libs/Pixy/src/chirpreceiver.hpp rename to discovery/libs/Pixy/src/chirpreceiver.hpp diff --git a/common/libs/Pixy/src/colorlut.cpp.notneeded b/discovery/libs/Pixy/src/colorlut.cpp.notneeded similarity index 100% rename from common/libs/Pixy/src/colorlut.cpp.notneeded rename to discovery/libs/Pixy/src/colorlut.cpp.notneeded diff --git a/common/libs/Pixy/src/colorlut.h b/discovery/libs/Pixy/src/colorlut.h similarity index 100% rename from common/libs/Pixy/src/colorlut.h rename to discovery/libs/Pixy/src/colorlut.h diff --git a/common/libs/Pixy/src/debug.h b/discovery/libs/Pixy/src/debug.h similarity index 100% rename from common/libs/Pixy/src/debug.h rename to discovery/libs/Pixy/src/debug.h diff --git a/common/libs/Pixy/src/debuglog.h b/discovery/libs/Pixy/src/debuglog.h similarity index 100% rename from common/libs/Pixy/src/debuglog.h rename to discovery/libs/Pixy/src/debuglog.h diff --git a/common/libs/Pixy/src/interpreter.hpp b/discovery/libs/Pixy/src/interpreter.hpp similarity index 100% rename from common/libs/Pixy/src/interpreter.hpp rename to discovery/libs/Pixy/src/interpreter.hpp diff --git a/common/libs/Pixy/src/link.h b/discovery/libs/Pixy/src/link.h similarity index 100% rename from common/libs/Pixy/src/link.h rename to discovery/libs/Pixy/src/link.h diff --git a/common/libs/Pixy/src/pixy.cpp b/discovery/libs/Pixy/src/pixy.cpp similarity index 100% rename from common/libs/Pixy/src/pixy.cpp rename to discovery/libs/Pixy/src/pixy.cpp diff --git a/common/libs/Pixy/src/pixyinterpreter.cpp b/discovery/libs/Pixy/src/pixyinterpreter.cpp similarity index 100% rename from common/libs/Pixy/src/pixyinterpreter.cpp rename to discovery/libs/Pixy/src/pixyinterpreter.cpp diff --git a/common/libs/Pixy/src/pixyinterpreter.hpp b/discovery/libs/Pixy/src/pixyinterpreter.hpp similarity index 100% rename from common/libs/Pixy/src/pixyinterpreter.hpp rename to discovery/libs/Pixy/src/pixyinterpreter.hpp diff --git a/common/libs/Pixy/src/pixytypes.h b/discovery/libs/Pixy/src/pixytypes.h similarity index 100% rename from common/libs/Pixy/src/pixytypes.h rename to discovery/libs/Pixy/src/pixytypes.h diff --git a/common/libs/Pixy/src/qqueue.cpp.notneeded b/discovery/libs/Pixy/src/qqueue.cpp.notneeded similarity index 100% rename from common/libs/Pixy/src/qqueue.cpp.notneeded rename to discovery/libs/Pixy/src/qqueue.cpp.notneeded diff --git a/common/libs/Pixy/src/qqueue.h b/discovery/libs/Pixy/src/qqueue.h similarity index 100% rename from common/libs/Pixy/src/qqueue.h rename to discovery/libs/Pixy/src/qqueue.h diff --git a/common/libs/Pixy/src/simplevector.h b/discovery/libs/Pixy/src/simplevector.h similarity index 100% rename from common/libs/Pixy/src/simplevector.h rename to discovery/libs/Pixy/src/simplevector.h diff --git a/common/libs/Pixy/src/usblink.cpp b/discovery/libs/Pixy/src/usblink.cpp similarity index 67% rename from common/libs/Pixy/src/usblink.cpp rename to discovery/libs/Pixy/src/usblink.cpp index 76893cc..02f4447 100644 --- a/common/libs/Pixy/src/usblink.cpp +++ b/discovery/libs/Pixy/src/usblink.cpp @@ -4,6 +4,16 @@ #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; diff --git a/common/libs/Pixy/src/usblink.h b/discovery/libs/Pixy/src/usblink.h similarity index 100% rename from common/libs/Pixy/src/usblink.h rename to discovery/libs/Pixy/src/usblink.h diff --git a/emulator/.gitignore b/emulator/.gitignore index 761dc34..c45d13f 100644 --- a/emulator/.gitignore +++ b/emulator/.gitignore @@ -6,3 +6,9 @@ qt/*.a qt/ui_* qt/moc_* qt/Makefile + + +libs/*/obj +libs/*/*.a +libs/*/*.o + diff --git a/emulator/Makefile b/emulator/Makefile index 88edad3..d3c1965 100644 --- a/emulator/Makefile +++ b/emulator/Makefile @@ -3,6 +3,8 @@ TARGET=emulator BUILD_DIR=./build OBJ_DIR=./obj +LIB_DIR=./libs + QT_DIR=./qt COMMON_DIR=../common @@ -17,8 +19,7 @@ MKDIR=mkdir -p RM=rm -f RMDIR=rm -rf -INCLUDES=$(shell find $(COMMON_DIR) -maxdepth 1 -type d ! -path $(COMMON_DIR) ! -path \*/libs) -INCLUDES+=$(COMMON_DIR)/libs/Pixy +INCLUDES=$(shell find $(COMMON_DIR) -maxdepth 1 -type d ! -path $(COMMON_DIR)) INCLUDES:=$(addprefix -I,$(INCLUDES)) QT_LIB=$(QT_DIR)/libemulatorqt.a @@ -27,8 +28,13 @@ CPPFLAGS= -march=x86-64 -mtune=generic -fPIC $(INCLUDES) CFLAGS= -O0 -g -LIBS=Qt5Core Qt5Gui Qt5Widgets emulatorqt m stdc++ +LIBS= pixy usb-1.0 boost_system boost_timer boost_chrono +LIBS+=Qt5Core Qt5Gui Qt5Widgets emulatorqt m stdc++ + + LDFLAGS= -L$(QT_DIR) $(addprefix -l,$(LIBS)) +LDFLAGS+= -L$(LIB_DIR)/Pixy + #Finding Input files CFILES=$(shell find . -maxdepth 1 -name '*.c') diff --git a/emulator/libs/Pixy/Makefile b/emulator/libs/Pixy/Makefile new file mode 100644 index 0000000..1f91b47 --- /dev/null +++ b/emulator/libs/Pixy/Makefile @@ -0,0 +1,56 @@ +#2015 by tmoe, id10101 (and the internet :) ) + +TARGET=libpixy + +#Tools +CC=g++ +AR=ar +RMDIR = rm -rf +RM=rm -f +MKDIR=mkdir -p + +#Directories +SRC_DIR=./src +INC_DIR=../../../common/pixy +OBJ_DIR=./obj + +#Architecture flags + +#Compiler, Linker Options +CPPFLAGS=-I$(INC_DIR) -D__LINUX__=1 -DHOST=1 -DDEBUG=1 +CFLAGS=$(ARCH_FLAGS) -O0 -g #-ffunction-sections -fdata-sections -g +#CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 -mthumb-interwork +#CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16 +CFLAGS+=-I/usr/include/libusb-1.0 + +#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 diff --git a/emulator/libs/Pixy/hello_pixy.cpp b/emulator/libs/Pixy/hello_pixy.cpp new file mode 100644 index 0000000..434a79e --- /dev/null +++ b/emulator/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 +#include +#include +#include +#include +#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(); +} diff --git a/emulator/libs/Pixy/src/blob.cpp.notneeded b/emulator/libs/Pixy/src/blob.cpp.notneeded new file mode 100644 index 0000000..8f6c943 --- /dev/null +++ b/emulator/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 +#ifdef PIXY +#include "pixy_init.h" +#include "exec.h" +#else +#include "debug.h" +#endif +#include "blob.h" + +#ifdef DEBUG +#ifndef HOST +#include +#else +#include +#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(¤tBlob->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(¤tBlob->next); +} + +void +CBlobAssembler::AdvanceCurrent() +{ + previousBlobPtr= &(currentBlob->next); + currentBlob= *previousBlobPtr; + if (currentBlob) BlobNewRow(¤tBlob->next); +} + + diff --git a/emulator/libs/Pixy/src/blob.h b/emulator/libs/Pixy/src/blob.h new file mode 100644 index 0000000..bc01357 --- /dev/null +++ b/emulator/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 +#include +//#include +#include + +//#define INCLUDE_STATS + +// Uncomment this for verbose output for testing +//#include + +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 diff --git a/emulator/libs/Pixy/src/blobs.cpp.notneeded b/emulator/libs/Pixy/src/blobs.cpp.notneeded new file mode 100644 index 0000000..88f16a8 --- /dev/null +++ b/emulator/libs/Pixy/src/blobs.cpp.notneeded @@ -0,0 +1,1084 @@ +// +// 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 +#include "debug.h" +#else +#include "pixy_init.h" +#endif + +#include "blobs.h" + +#define CC_SIGNATURE(s) (m_ccMode==CC_ONLY || m_clut.getType(s)==CL_MODEL_TYPE_COLORCODE) + +Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut) +{ + int i; + + m_mutex = false; + m_minArea = MIN_AREA; + m_maxBlobs = MAX_BLOBS; + m_maxBlobsPerModel = MAX_BLOBS_PER_MODEL; + m_mergeDist = MAX_MERGE_DIST; + m_maxBlob = NULL; + + m_qq = qq; +#ifdef PIXY + m_maxCodedDist = MAX_CODED_DIST; +#else + m_maxCodedDist = MAX_CODED_DIST/2; + m_qvals = new uint32_t[0x8000]; +#endif + m_ccMode = DISABLED; + + m_blobs = new uint16_t[MAX_BLOBS*5]; + m_numBlobs = 0; + m_blobReadIndex = 0; + m_ccBlobReadIndex = 0; + + // reset blob assemblers + for (i=0; idequeue(&qval)==0); + if (qval.m_col>=0xfffe) + break; + if (res<0) + continue; + if (qval.m_col==0) + { + prevStartCol = 0xffff; + prevSig = 0; + if (segmentSig) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + row++; +#ifndef PIXY + m_qvals[m_numQvals++] = 0; +#else + if (icount++==5) // an interleave of every 5 lines or about every 175us seems good + { + g_chirpUsb->service(); + icount = 0; + } +#endif + continue; + } + + sig = qval.m_col&0x07; + + u = qval.m_u; + v = qval.m_v; + + u <<= CL_LUT_ENTRY_SCALE; + v <<= CL_LUT_ENTRY_SCALE; + c = qval.m_y; + if (c==0) + c = 1; + u /= c; + v /= c; + + if (m_clut.m_runtimeSigs[sig-1].m_uMin=(int32_t)m_clut.m_miny) + { + qval.m_col >>= 3; + startCol = qval.m_col; + merge = startCol-prevStartCol<=5 && prevSig==sig; + if (segmentSig==0 && merge) + { + segmentSig = sig; + segmentStartCol = prevStartCol; + } + else if (segmentSig!=0 && (segmentSig!=sig || !merge)) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + + if (segmentSig!=0 && merge) + segmentEndCol = startCol; + else if (segmentSig==0 && !merge) + res = handleSegment(sig, row, startCol-1, 2); + prevSig = sig; + prevStartCol = startCol; + } + else if (segmentSig!=0) + { + res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1); + segmentSig = 0; + } + } + endFrame(); + + if (qval.m_col==0xfffe) // error code, queue overrun + return -1; + return 0; +} + +int Blobs::blobify() +{ + uint32_t i, j, k; + bool colorCode; + CBlob *blob; + uint16_t *blobsStart; + uint16_t numBlobsStart, invalid, invalid2; + uint16_t left, top, right, bottom; + //uint32_t timer, timer2=0; + + if (runlengthAnalysis()<0) + { + for (i=0; inext, k++) + { + if ((colorCode && blob->GetArea()GetArea()<(int)m_minArea)) + continue; + blob->getBBox((short &)left, (short &)top, (short &)right, (short &)bottom); + if (bottom-top<=1) // blobs that are 1 line tall + continue; + m_blobs[j + 0] = i+1; + m_blobs[j + 1] = left; + m_blobs[j + 2] = right; + m_blobs[j + 3] = top; + m_blobs[j + 4] = bottom; + m_numBlobs++; + j += 5; + + } + //setTimer(&timer); + if (!colorCode) // do not combine color code models + { + while(1) + { + invalid2 = combine2(blobsStart, m_numBlobs-numBlobsStart); + if (invalid2==0) + break; + invalid += invalid2; + } + } + //timer2 += getTimer(timer); + } + //setTimer(&timer); + invalid += combine(m_blobs, m_numBlobs); + if (m_ccMode!=DISABLED) + { + m_ccBlobs = (BlobB *)(m_blobs + m_numBlobs*5); + // calculate number of codedblobs left + processCC(); + } + if (invalid || m_ccMode!=DISABLED) + { + invalid2 = compress(m_blobs, m_numBlobs); + m_numBlobs -= invalid2; + } + //timer2 += getTimer(timer); + //cprintf("time=%d\n", timer2); // never seen this greater than 200us. or 1% of frame period + + // reset read indexes-- new frame + m_blobReadIndex = 0; + m_ccBlobReadIndex = 0; + m_mutex = false; + + // free memory + for (i=0; i0) + cprintf("%d: blobs %d %d %d %d %d\n", frame, m_numBlobs, m_blobs[1], m_blobs[2], m_blobs[3], m_blobs[4]); + else + cprintf("%d: blobs 0\n", frame); + frame++; +#endif + return 0; +} + +#ifndef PIXY +void Blobs::getRunlengths(uint32_t **qvals, uint32_t *len) +{ + *qvals = m_qvals; + *len = m_numQvals; +} +#endif + +uint16_t Blobs::getCCBlock(uint8_t *buf, uint32_t buflen) +{ + uint16_t *buf16 = (uint16_t *)buf; + uint16_t temp, width, height; + uint16_t checksum; + uint16_t len = 8; // default + + if (buflen<9*sizeof(uint16_t)) + return 0; + + if (m_mutex || m_ccBlobReadIndex>=m_numCCBlobs) // we're copying, so no CC blocks for now.... + return 0; + + if (m_blobReadIndex==0 && m_ccBlobReadIndex==0) // beginning of frame, mark it with empty block + { + buf16[0] = BL_BEGIN_MARKER; + len++; + buf16++; + } + + // beginning of block + buf16[0] = BL_BEGIN_MARKER_CC; + + // model + temp = m_ccBlobs[m_ccBlobReadIndex].m_model; + checksum = temp; + buf16[2] = temp; + + // width + width = m_ccBlobs[m_ccBlobReadIndex].m_right - m_ccBlobs[m_ccBlobReadIndex].m_left; + checksum += width; + buf16[5] = width; + + // height + height = m_ccBlobs[m_ccBlobReadIndex].m_bottom - m_ccBlobs[m_ccBlobReadIndex].m_top; + checksum += height; + buf16[6] = height; + + // x center + temp = m_ccBlobs[m_ccBlobReadIndex].m_left + width/2; + checksum += temp; + buf16[3] = temp; + + // y center + temp = m_ccBlobs[m_ccBlobReadIndex].m_top + height/2; + checksum += temp; + buf16[4] = temp; + + temp = m_ccBlobs[m_ccBlobReadIndex].m_angle; + checksum += temp; + buf16[7] = temp; + + buf16[1] = checksum; + + // next blob + m_ccBlobReadIndex++; + + return len*sizeof(uint16_t); +} + + +uint16_t Blobs::getBlock(uint8_t *buf, uint32_t buflen) +{ + uint16_t *buf16 = (uint16_t *)buf; + uint16_t temp, width, height; + uint16_t checksum; + uint16_t len = 7; // default + int i = m_blobReadIndex*5; + + if (buflen<8*sizeof(uint16_t)) + return 0; + + if (m_blobReadIndex>=m_numBlobs && m_ccMode!=DISABLED) + return getCCBlock(buf, buflen); + + if (m_mutex || m_blobReadIndex>=m_numBlobs) // we're copying, so no blocks for now.... + return 0; + + if (m_blobReadIndex==0) // beginning of frame, mark it with empty block + { + buf16[0] = BL_BEGIN_MARKER; + len++; + buf16++; + } + + // beginning of block + buf16[0] = BL_BEGIN_MARKER; + + // model + temp = m_blobs[i]; + checksum = temp; + buf16[2] = temp; + + // width + width = m_blobs[i+2] - m_blobs[i+1]; + checksum += width; + buf16[5] = width; + + // height + height = m_blobs[i+4] - m_blobs[i+3]; + checksum += height; + buf16[6] = height; + + // x center + temp = m_blobs[i+1] + width/2; + checksum += temp; + buf16[3] = temp; + + // y center + temp = m_blobs[i+3] + height/2; + checksum += temp; + buf16[4] = temp; + + buf16[1] = checksum; + + // next blob + m_blobReadIndex++; + + return len*sizeof(uint16_t); +} + + +BlobA *Blobs::getMaxBlob(uint16_t signature) +{ + int i, j; + uint32_t area, maxArea; + BlobA *blob; + BlobB *ccBlob; + + if (signature==0) // 0 means return the biggest regardless of signature number + { + // if we've already found it, return it + if (m_maxBlob) + return m_maxBlob; + + // look through all blobs looking for the blob with the biggest area + for (i=0, maxArea=0; im_right - blob->m_left)*(blob->m_bottom - blob->m_top); + if (area>maxArea) + { + maxArea = area; + m_maxBlob = blob; + } + } + for (i=0; im_right - ccBlob->m_left)*(ccBlob->m_bottom - ccBlob->m_top); + if (area>maxArea) + { + maxArea = area; + m_maxBlob = (BlobA *)ccBlob; + } + } + return m_maxBlob; + } + else + { + for (i=0, j=0; i=right && top0<=top && bottom0>=bottom) + { + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (left<=left0 && right>=right0 && top<=top0 && bottom>=bottom0) + { + blobs[ii+0] = 0; // invalidate + invalid++; + } + } + } + + return invalid; +} + +uint16_t Blobs::combine2(uint16_t *blobs, uint16_t numBlobs) +{ + uint16_t i, j, ii, jj, left0, right0, top0, bottom0; + uint16_t left, right, top, bottom; + uint16_t invalid; + + for (i=0, ii=0, invalid=0; i=right0 && left-right0<=m_mergeDist && + ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0))) + { + blobs[ii+2] = right; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (top<=top0 && top0-bottom<=m_mergeDist && + ((left0<=left && left<=right0) || (left0<=right && right<=right0))) + { + blobs[ii+3] = top; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && + ((left0<=left && left<=right0) || (left0<=right && right<=right0))) + { + blobs[ii+4] = bottom; + blobs[jj+0] = 0; // invalidate + invalid++; + } +#else // at least half of a side (the smaller adjacent side) has to overlap + if (left<=left0 && left0-right<=m_mergeDist && + ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) + { + blobs[ii+1] = left; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (right>=right0 && left-right0<=m_mergeDist && + ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom))) + { + blobs[ii+2] = right; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (top<=top0 && top0-bottom<=m_mergeDist && + ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) + { + blobs[ii+3] = top; + blobs[jj+0] = 0; // invalidate + invalid++; + } + else if (bottom>=bottom0 && top-bottom0<=m_mergeDist && + ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right))) + { + blobs[ii+4] = bottom; + blobs[jj+0] = 0; // invalidate + invalid++; + } +#endif + } + } + + return invalid; +} + +int16_t Blobs::distance(BlobA *blob0, BlobA *blob1) +{ + int16_t left0, right0, top0, bottom0; + int16_t left1, right1, top1, bottom1; + + left0 = blob0->m_left; + right0 = blob0->m_right; + top0 = blob0->m_top; + bottom0 = blob0->m_bottom; + left1 = blob1->m_left; + right1 = blob1->m_right; + top1 = blob1->m_top; + bottom1 = blob1->m_bottom; + + if (left0>=left1 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) + return left0-right1; + + if (left1>=left0 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0)))) + return left1-right0; + + if (top0>=top1 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) + return top0-bottom1; + + if (top1>=top0 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0)))) + return top1-bottom0; + + return 0x7fff; // return a large number +} + +bool Blobs::closeby(BlobA *blob0, BlobA *blob1) +{ + // check to see if blobs are invalid or equal + if (blob0->m_model==0 || blob1->m_model==0 || blob0->m_model==blob1->m_model) + return false; + // check to see that the blobs are from color code models. If they aren't both + // color code blobs, we return false + if (!CC_SIGNATURE(blob0->m_model&0x07) || !CC_SIGNATURE(blob1->m_model&0x07)) + return false; + + return distance(blob0, blob1)<=m_maxCodedDist; +} + +int16_t Blobs::distance(BlobA *blob0, BlobA *blob1, bool horiz) +{ + int16_t dist; + + if (horiz) + dist = (blob0->m_right+blob0->m_left)/2 - (blob1->m_right+blob1->m_left)/2; + else + dist = (blob0->m_bottom+blob0->m_top)/2 - (blob1->m_bottom+blob1->m_top)/2; + + if (dist<0) + return -dist; + else + return dist; +} + +int16_t Blobs::angle(BlobA *blob0, BlobA *blob1) +{ + int acx, acy, bcx, bcy; + float res; + + acx = (blob0->m_right + blob0->m_left)/2; + acy = (blob0->m_bottom + blob0->m_top)/2; + bcx = (blob1->m_right + blob1->m_left)/2; + bcy = (blob1->m_bottom + blob1->m_top)/2; + + res = atan2((float)(acy-bcy), (float)(bcx-acx))*180/3.1415f; + + return (int16_t)res; +} + +void Blobs::sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz) +{ + uint16_t i, td, distances[MAX_COLOR_CODE_MODELS*2]; + bool done; + BlobA *tb; + + // create list of distances + for (i=0; idistances[i]) + { + // swap distances + td = distances[i]; + distances[i] = distances[i-1]; + distances[i-1] = td; + // swap blobs + tb = blobs[i]; + blobs[i] = blobs[i-1]; + blobs[i-1] = tb; + + done = false; + } + } + if (done) + break; + } +} + +bool Blobs::analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB) +{ + bool skip; + bool result = false; + int16_t dist, minDist, i, j, k; + + for (i=0, minDist=0x7fff; im_model&0x07)==(blobs[j]->m_model&0x07)) + { + skip = true; + break; + } + } + if (skip) + continue; + dist = distance(blobs0[i], blobs[j]); + if (distm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); + lowerArea = (area0*100)/(100+TOL); + upperArea = area0 + (area0*TOL)/100; + + for (j=0, numEqual=0; j<*numBlobs; j++) + { + if (i==j) + continue; + area1 = (blobs[j]->m_right-blobs[j]->m_left) * (blobs[j]->m_bottom-blobs[j]->m_top); + if (lowerArea<=area1 && area1<=upperArea) + numEqual++; + } + if (numEqual>maxEqual) + { + maxEqual = numEqual; + maxEqualArea = area0; + set = true; + } + } + + if (!set) + *numBlobs = 0; + + for (i=0, numNewBlobs=0; i<*numBlobs && numNewBlobsm_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top); + lowerArea = (area0*100)/(100+TOL); + upperArea = area0 + (area0*TOL)/100; + if (lowerArea<=maxEqualArea && maxEqualArea<=upperArea) + newBlobs[numNewBlobs++] = blobs[i]; +#ifndef PIXY + else if (*numBlobs>=5 && (blobs[i]->m_model&0x07)==2) + DBG("eliminated!"); +#endif + } + + // copy new blobs over + for (i=0; im_model&0x07)==(blobs[i]->m_model&0x07)) + set = true; + else + break; + } + } + if (set) + { + // copy new blobs over + for (i=0; im_model<=CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) + { + count++; + scount = count<<3; + blob0->m_model |= scount; + blob1->m_model |= scount; + } + else if (blob0->m_model>CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES) + { + scount = blob0->m_model & ~0x07; + blob1->m_model |= scount; + } + else if (blob1->m_model>CL_NUM_SIGNATURES && blob0->m_model<=CL_NUM_SIGNATURES) + { + scount = blob1->m_model & ~0x07; + blob0->m_model |= scount; + } + } + } + } + +#if 1 + // 2nd pass: merge blob clumps + for (blob0=(BlobA *)m_blobs; blob0m_model<=CL_NUM_SIGNATURES) // skip normal blobs + continue; + scount = blob0->m_model&~0x07; + for (blob1=(BlobA *)blob0+1; blob1m_model<=CL_NUM_SIGNATURES) + continue; + + scount1 = blob1->m_model&~0x07; + if (scount!=scount1 && closeby(blob0, blob1)) + mergeClumps(scount, scount1); + } + } +#endif + + // 3rd and final pass, find each blob clean it up and add it to the table + endBlobB = (BlobB *)((BlobA *)m_blobs + MAX_BLOBS)-1; + for (i=1, codedBlob = m_ccBlobs, m_numCCBlobs=0; i<=count && codedBlobm_model&~0x07)==scount) + blobs[j++] = blob0; + } + +#if 1 + // cleanup blobs, deal with cases where there are more blobs than models + cleanup(blobs, &j); +#endif + + if (j<2) + continue; + + // find left, right, top, bottom of color coded block + for (k=0, left=right=top=bottom=avgWidth=avgHeight=0; km_model, blobs[k]->m_left, blobs[k]->m_right, blobs[k]->m_top, blobs[k]->m_bottom); + if (blobs[left]->m_left > blobs[k]->m_left) + left = k; + if (blobs[top]->m_top > blobs[k]->m_top) + top = k; + if (blobs[right]->m_right < blobs[k]->m_right) + right = k; + if (blobs[bottom]->m_bottom < blobs[k]->m_bottom) + bottom = k; + avgWidth += blobs[k]->m_right - blobs[k]->m_left; + avgHeight += blobs[k]->m_bottom - blobs[k]->m_top; + } + avgWidth /= j; + avgHeight /= j; + codedBlob->m_left = blobs[left]->m_left; + codedBlob->m_right = blobs[right]->m_right; + codedBlob->m_top = blobs[top]->m_top; + codedBlob->m_bottom = blobs[bottom]->m_bottom; + +#if 1 + // is it more horizontal than vertical? + width = (blobs[right]->m_right - blobs[left]->m_left)*100; + width /= avgWidth; // scale by average width because our swatches might not be square + height = (blobs[bottom]->m_bottom - blobs[top]->m_top)*100; + height /= avgHeight; // scale by average height because our swatches might not be square + + if (width > height) + sort(blobs, j, blobs[left], true); + else + sort(blobs, j, blobs[top], false); + +#if 1 + cleanup2(blobs, &j); + if (j<2) + continue; + else if (j>5) + j = 5; +#endif + // create new blob, compare the coded models, pick the smaller one + for (k=0, codedModel0=0; km_model&0x07; + } + for (k=j-1, codedModel=0; k>=0; k--) + { + codedModel <<= 3; + codedModel |= blobs[k]->m_model&0x07; + blobs[k]->m_model = 0; // invalidate + } + + if (codedModel0m_model = codedModel0; + codedBlob->m_angle = angle(blobs[0], blobs[j-1]); + } + else + { + codedBlob->m_model = codedModel; + codedBlob->m_angle = angle(blobs[j-1], blobs[0]); + } +#endif + //DBG("cc %d %d %d %d %d", m_numCCBlobs, codedBlob->m_left, codedBlob->m_right, codedBlob->m_top, codedBlob->m_bottom); + codedBlob++; + m_numCCBlobs++; + } + + // 3rd pass, invalidate blobs + for (blob0=(BlobA *)m_blobs; blob0m_model>CL_NUM_SIGNATURES) + blob0->m_model = 0; + } + else if (blob0->m_model>CL_NUM_SIGNATURES || CC_SIGNATURE(blob0->m_model)) + blob0->m_model = 0; // invalidate-- not part of a color code + } +} + +void Blobs::endFrame() +{ + int i; + for (i=0; i +#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 diff --git a/emulator/libs/Pixy/src/calc.cpp.notneeded b/emulator/libs/Pixy/src/calc.cpp.notneeded new file mode 100644 index 0000000..06696a5 --- /dev/null +++ b/emulator/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); +} diff --git a/emulator/libs/Pixy/src/calc.h b/emulator/libs/Pixy/src/calc.h new file mode 100644 index 0000000..648db38 --- /dev/null +++ b/emulator/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 + +#ifdef MAX +#undef MAX +#endif +#ifdef MIN +#undef MIN +#endif +#define MAX(a, b) (a>b ? a : b) +#define MIN(a, b) (a +#include +#include "chirp.hpp" +#include "debuglog.h" + + +// todo yield, sleep() while waiting for sync response +// todo + +// assume that destination is aligned on the correct boundary and copy the source byte by byte +void copyAlign(char *dest, const char *src, int size) +{ + int i; + for (i=0; igetFlags()&LINK_FLAG_ERROR_CORRECTED; + m_sharedMem = m_link->getFlags()&LINK_FLAG_SHARED_MEM; + m_blkSize = m_link->blockSize(); + + if (m_errorCorrected) + m_headerLen = 12; // startcode (uint32_t), type (uint8_t), (pad), proc (uint16_t), len (uint32_t) + else + m_headerLen = 8; // type (uint8_t), (pad), proc (uint16_t), len (uint32_t) + + if (m_sharedMem) + { + m_buf = (uint8_t *)m_link->getFlags(LINK_FLAG_INDEX_SHARED_MEMORY_LOCATION); + m_bufSize = m_link->getFlags(LINK_FLAG_INDEX_SHARED_MEMORY_SIZE); + } + else + { + m_bufSize = CRP_BUFSIZE; + m_buf = new (std::nothrow) uint8_t[m_bufSize]; + } + + // link is set up, need to call init + if (m_client) { + return_value = remoteInit(true); + log("pixydebug: remoteInit() = %d\n", return_value); + log("pixydebug: setLink() returned %d\n", return_value); + return return_value; + } + + log("pixydebug: setLink() returned %d\n", CRP_RES_OK); + return CRP_RES_OK; +} + + +int Chirp::assemble(uint8_t type, ...) +{ + int res; + va_list args; + bool save = m_call; + uint32_t saveLen = m_len; + + if (type==CRP_XDATA) + m_call = false; + + va_start(args, type); + res = vassemble(&args); + va_end(args); + + if (type==CRP_XDATA || (!m_call && res==CRP_RES_OK)) // if we're not a call, we're extra data, so we need to send + { + res = sendChirpRetry(CRP_XDATA, 0); + m_len = saveLen; + } + + m_call = save; + + return res; +} + +int Chirp::vassemble(va_list *args) +{ + int len; + + len = vserialize(this, m_buf, m_bufSize, args); + // check for error + if (len<0) + return len; + + // set length (don't include header) + m_len = len - m_headerLen; + + return CRP_RES_OK; +} + +bool Chirp::connected() +{ + return m_connected; +} + +int Chirp::useBuffer(uint8_t *buf, uint32_t len) +{ + int res; + + if (m_bufSave==NULL) + { + m_bufSave = m_buf; + m_buf = buf; + } + else if (buf!=m_buf) + return CRP_RES_ERROR_MEMORY; + + m_len = len-m_headerLen; + if (!m_call) // if we're not a call, we're extra data, so we need to send + { + res = sendChirpRetry(CRP_XDATA, 0); + restoreBuffer(); // restore buffer immediately! + if (res!=CRP_RES_OK) // convert call into response + return res; + } + return CRP_RES_OK; +} + +void Chirp::restoreBuffer() +{ + if (m_bufSave) + { + m_buf = m_bufSave; + m_bufSave = NULL; + } +} + + +int Chirp::serialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, ...) +{ + int res; + va_list args; + + va_start(args, bufSize); + res = vserialize(chirp, buf, bufSize, &args); + va_end(args); + + return res; +} + +#define RESIZE_BUF(size) \ + if (size > bufSize-CRP_BUFPAD) \ +{ \ + if (!chirp) \ + return CRP_RES_ERROR_MEMORY; \ + else \ +{ \ + if ((res=chirp->realloc(size))<0) \ + return res; \ + buf = chirp->m_buf; \ + bufSize = chirp->m_bufSize; \ + } \ + } \ + + +int Chirp::vserialize(Chirp *chirp, uint8_t *buf, uint32_t bufSize, va_list *args) +{ + int res; + uint8_t type, origType; + uint32_t i, si; + bool copy = true; + + if (chirp) + { + if (chirp->m_call) // reserve an extra 4 for responseint + i = chirp->m_headerLen+4; + else // if it's a chirp call, just reserve the header + i = chirp->m_headerLen; + } + else + i = 0; + + bufSize -= i; + + while(1) + { +#if 1 + type = va_arg(*args, int); +#else + type = va_arg(*args, uint8_t); +#endif + + if (type==END) + break; + + si = i; // save index so we can skip over data if needed + buf[i++] = type; + + // treat hints like other types for now + // but if gotoe (guy on the other end) isn't interested in hints (m_hinformer=false), + // we'll restore index to si and effectively skip data. + origType = type; + type &= ~CRP_HINT; + + if (type==CRP_INT8) + { +#if 1 + int8_t val = va_arg(*args, int); +#else + int8_t val = va_arg(*args, int8_t); +#endif + *(int8_t *)(buf+i) = val; + i += 1; + } + else if (type==CRP_INT16) + { +#if 1 + int16_t val = va_arg(*args, int); +#else + int16_t val = va_arg(*args, int16_t); +#endif + ALIGN(i, 2); + // rewrite type so getType will work (even though we might add padding between type and data) + buf[i-1] = origType; + *(int16_t *)(buf+i) = val; + i += 2; + } + else if (type==CRP_INT32 || origType==CRP_TYPE_HINT) // CRP_TYPE_HINT is a special case... + { + int32_t val = va_arg(*args, int32_t); + ALIGN(i, 4); + buf[i-1] = origType; + *(int32_t *)(buf+i) = val; + i += 4; + } + else if (type==CRP_FLT32) + { +#if 1 + float val = va_arg(*args, double); +#else + float val = va_arg(*args, float); +#endif + ALIGN(i, 4); + buf[i-1] = origType; + *(float *)(buf+i) = val; + i += 4; + } + else if (type==CRP_STRING) + { + int8_t *s = va_arg(*args, int8_t *); + uint32_t len = strlen((char *)s)+1; // include null + + RESIZE_BUF(len+i); + + memcpy(buf+i, s, len); + i += len; + } + else if (type&CRP_ARRAY) + { + uint8_t size = type&0x0f; + uint32_t len = va_arg(*args, int32_t); + + // deal with no copy case (use our own buffer) + if ((type&CRP_NO_COPY)==CRP_NO_COPY) + { + // rewrite type so as not to confuse gotoe + origType = type &= ~CRP_NO_COPY; + buf[i-1] = origType; + copy = false; + } + + ALIGN(i, 4); + buf[i-1] = origType; + *(uint32_t *)(buf+i) = len; + i += 4; + ALIGN(i, size); + + if (copy) + { + len *= size; // scale by size of array elements + + RESIZE_BUF(len+i); + + int8_t *ptr = va_arg(*args, int8_t *); + memcpy(buf+i, ptr, len); + i += len; + } + } + else + return CRP_RES_ERROR_PARSE; + + // skip hint data if we're not a source + if (chirp && !chirp->m_hinformer && origType&CRP_HINT) + i = si; + + RESIZE_BUF(i); + } + + // return length + return i; +} + + +// this isn't completely necessary, but it makes things a lot easier to use. +// passing a pointer to a pointer and then having to dereference is just confusing.... +// so for scalars (ints, floats) you don't need to pass in ** pointers, just * pointers so +// chirp can write the value into the * pointer and hand it back. +// But for arrays you need ** pointers, so chirp doesn't need to copy the whole array into your buffer--- +// chirp will write the * pointer value into your ** pointer. +int Chirp::loadArgs(va_list *args, void *recvArgs[]) +{ + int i; + uint8_t type, size; + void **recvArg; + + for (i=0; recvArgs[i]!=NULL && isetTimer(); // set timer, so we can check to see if we're taking too much time + + while(1) + { + if ((res=recvChirp(&type, &recvProc, recvArgs, true))==CRP_RES_OK) + { + if (type&CRP_RESPONSE) + break; + else // handle calls as they come in + handleChirp(type, recvProc, (const void **)recvArgs); + } + else + { + va_end(arguments); + return res; + } + if (m_link->getTimer()>m_headerTimeout) // we could receive XDATA (for example) and never exit this while loop + return CRP_RES_ERROR_RECV_TIMEOUT; + } + + // deal with arguments + if (service&RETURN_ARRAY) // copy array of arguments + { + void **recvArray; + while(1) + { + recvArray = va_arg(arguments, void **); + if (recvArray!=NULL) + break; + } + for (i=0; recvArgs[i]; i++) + recvArray[i] = recvArgs[i]; + recvArray[i] = NULL; + } + else if ((res=loadArgs(&arguments, recvArgs))<0) + { + va_end(arguments); + return res; + } + } + + + va_end(arguments); + return CRP_RES_OK; +} + +int Chirp::call(uint8_t service, ChirpProc proc, ...) +{ + int result; + + va_list arguments; + + va_start(arguments, proc); + result = call(service, proc, arguments); + va_end(arguments); + + return result; +} + +int Chirp::sendChirpRetry(uint8_t type, ChirpProc proc) +{ + int i, res=-1; + + if (!m_connected && !(type&CRP_INTRINSIC)) + return CRP_RES_ERROR_NOT_CONNECTED; + // deal with case where there is no actual data (e.g. it's all hint data and gotoe isn't hinterested) + // but chirp calls can have no data of course + if (m_len==0 && !(type&CRP_CALL)) + return CRP_RES_OK; + for (i=0; i=m_procTableSize || proc<0) + return CRP_RES_ERROR; // index exceeded + + ProcPtr ptr = m_procTable[proc].procPtr; + if (ptr==NULL) + return CRP_RES_ERROR; // some chirps are not meant to be called in both directions + + // count args + for (n=0; args[n]!=NULL; n++); + + m_call = true; // indicate to ourselves that this is a chirp call + // this is probably overkill.... + if (n==0) + responseInt = (*ptr)(this); + else if (n==1) + responseInt = (*(uint32_t(*)(const void*,Chirp*))ptr)(args[0],this); + else if (n==2) + responseInt = (*(uint32_t(*)(const void*,const void*,Chirp*))ptr)(args[0],args[1],this); + else if (n==3) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],this); + else if (n==4) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],this); + else if (n==5) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],this); + else if (n==6) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],this); + else if (n==7) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],this); + else if (n==8) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],this); + else if (n==9) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],args[8],this); + else if (n==10) + responseInt = (*(uint32_t(*)(const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,const void*,Chirp*))ptr)(args[0],args[1],args[2],args[3],args[4],args[5],args[6],args[7],args[8],args[9],this); + else + responseInt = CRP_RES_ERROR; + m_call = false; + } + + // if it's a chirp call, we need to send back the result + // result is in m_buf + if (type&CRP_CALL) + { + // write responseInt + *(uint32_t *)(m_buf+m_headerLen) = responseInt; + // send response + res = sendChirpRetry(CRP_RESPONSE | (type&~CRP_CALL), m_procTable[proc].chirpProc); // convert call into response + restoreBuffer(); // restore buffer immediately! + if (res!=CRP_RES_OK) + return res; + } + + return CRP_RES_OK; +} + +int Chirp::reallocTable() +{ + ProcTableEntry *newProcTable; + int newProcTableSize; + + // allocate new table, zero + newProcTableSize = m_procTableSize+CRP_PROCTABLE_LEN; + newProcTable = new (std::nothrow) ProcTableEntry[newProcTableSize]; + if (newProcTable==NULL) + return CRP_RES_ERROR_MEMORY; + memset(newProcTable, 0, sizeof(ProcTableEntry)*newProcTableSize); + // copy to new table + memcpy(newProcTable, m_procTable, sizeof(ProcTableEntry)*m_procTableSize); + // delete old table + delete [] m_procTable; + // set to new + m_procTable = newProcTable; + m_procTableSize = newProcTableSize; + + return CRP_RES_OK; +} + +ChirpProc Chirp::lookupTable(const char *procName) +{ + ChirpProc i; + + for(i=0; i=0) + return res; + + // a negative ChirpProc is an error + return -1; +} + +int Chirp::remoteInit(bool connect) +{ + int res; + uint32_t responseInt; + uint8_t hinformer; + + res = call(CRP_CALL_INIT, 0, + UINT16(connect ? m_blkSize : 0), // send block size + UINT8(m_hinterested), // send whether we're interested in hints or not + END_OUT_ARGS, + &responseInt, + &hinformer, // receive whether we should send hints + END_IN_ARGS + ); + if (res>=0) + { + m_connected = connect; + m_hinformer = hinformer; + return responseInt; + } + return res; +} + +int Chirp::getProcInfo(ChirpProc proc, ProcInfo *info) +{ + uint32_t responseInt; + int res; + + res = call(CRP_CALL_ENUMERATE_INFO, 0, + UINT16(proc), + END_OUT_ARGS, + &responseInt, + &info->procName, + &info->argTypes, + &info->procInfo, + END_IN_ARGS + ); + + if (res==CRP_RES_OK) + return responseInt; + return res; +} + +int Chirp::setProc(const char *procName, ProcPtr proc, ProcTableExtension *extension) +{ + ChirpProc cProc = updateTable(procName, proc); + if (cProc<0) + return CRP_RES_ERROR; + + m_procTable[cProc].extension = extension; + return CRP_RES_OK; +} + +int Chirp::registerModule(const ProcModule *module) +{ + int i; + + for (i=0; module[i].procName; i++) + setProc(module[i].procName, module[i].procPtr, (ProcTableExtension *)module[i].argTypes); + + return CRP_RES_OK; +} + +void Chirp::setSendTimeout(uint32_t timeout) +{ + m_sendTimeout = timeout; +} + +void Chirp::setRecvTimeout(uint32_t timeout) +{ + m_headerTimeout = timeout; +} + +int32_t Chirp::handleEnumerate(char *procName, ChirpProc *callback) +{ + ChirpProc proc; + // lookup in table + proc = lookupTable(procName); + // set remote index in table + m_procTable[proc].chirpProc = *callback; + + return proc; +} + +int32_t Chirp::handleInit(uint16_t *blkSize, uint8_t *hinformer) +{ + int32_t responseInt; + + bool connect = *blkSize ? true : false; + responseInt = init(connect); + m_connected = connect; + m_blkSize = *blkSize; // get block size, write it + m_hinformer = *hinformer; + + CRP_RETURN(this, UINT8(m_hinterested), END); + + return responseInt; +} + +int32_t Chirp::handleEnumerateInfo(ChirpProc *proc) +{ + const ProcTableExtension *extension; + uint8_t null = '\0'; + + if (*proc>=m_procTableSize || m_procTable[*proc].procName==NULL) + extension = NULL; + else + extension = m_procTable[*proc].extension; + + if (extension) + { + CRP_RETURN(this, STRING(m_procTable[*proc].procName), STRING(extension->argTypes), + STRING(extension->procInfo), END); + return CRP_RES_OK; + } + else // no extension, just send procedure name + { + CRP_RETURN(this, STRING(m_procTable[*proc].procName), STRING(&null), + STRING(&null), END); + return CRP_RES_ERROR; + } +} + +int Chirp::realloc(uint32_t min) +{ + if (m_sharedMem) + return CRP_RES_ERROR_MEMORY; + + if (!min) + min = m_bufSize+CRP_BUFSIZE; + else + min += CRP_BUFSIZE; + uint8_t *newbuf = new (std::nothrow) uint8_t[min]; + if (newbuf==NULL) + return CRP_RES_ERROR_MEMORY; + memcpy(newbuf, m_buf, m_bufSize); + delete[] m_buf; + m_buf = newbuf; + m_bufSize = min; + + return CRP_RES_OK; +} + +// service deals with calls and callbacks +int Chirp::service(bool all) +{ + int i; + uint8_t type; + ChirpProc recvProc; + void *args[CRP_MAX_ARGS+1]; + + for (i=0; true; i++) + { + if (recvChirp(&type, &recvProc, args)==CRP_RES_OK) + handleChirp(type, recvProc, (const void **)args); + else + break; + if (!all) + break; + } + + return i; +} + +int Chirp::recvChirp(uint8_t *type, ChirpProc *proc, void *args[], bool wait) // null pointer terminates +{ + int res; + uint32_t i, offset; + + restoreBuffer(); + + // receive + if (m_errorCorrected) + res = recvFull(type, proc, wait); + else + { + for (i=0; true; i++) + { + res = recvHeader(type, proc, wait); + if (res==CRP_RES_ERROR_CRC) + { + if (isend(m_buf, CRP_MAX_HEADER_LEN, m_sendTimeout))<0) + return res; + // if we haven't sent everything yet.... + if (m_len+m_headerLen>CRP_MAX_HEADER_LEN && !m_sharedMem) + { + if ((res=m_link->send(m_buf+CRP_MAX_HEADER_LEN, m_len-(CRP_MAX_HEADER_LEN-m_headerLen), m_sendTimeout))<0) + return res; + } + return CRP_RES_OK; +} + +int Chirp::sendHeader(uint8_t type, ChirpProc proc) +{ + int res; + bool ack; + uint32_t chunk, startCode = CRP_START_CODE; + uint16_t crc; + + if ((res=m_link->send((uint8_t *)&startCode, 4, m_sendTimeout))<0) + return res; + + *(uint8_t *)m_buf = type; + *(uint16_t *)(m_buf+2) = proc; + *(uint32_t *)(m_buf+4) = m_len; + if ((res=m_link->send(m_buf, m_headerLen, m_sendTimeout))<0) + return res; + crc = calcCrc(m_buf, m_headerLen); + + if (m_len>=CRP_MAX_HEADER_LEN) + chunk = CRP_MAX_HEADER_LEN; + else + chunk = m_len; + if (m_link->send(m_buf, chunk, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + + // send crc + crc += calcCrc(m_buf, chunk); + if (m_link->send((uint8_t *)&crc, 2, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + + if ((res=recvAck(&ack, m_headerTimeout))<0) + return res; + + if (ack) + m_offset = chunk; + else + return CRP_RES_ERROR_CRC; + + return CRP_RES_OK; +} + +int Chirp::sendData() +{ + uint16_t crc; + uint32_t chunk; + uint8_t sequence; + bool ack; + int res; + + for (sequence=0; m_offset=m_blkSize) + chunk = m_blkSize; + else + chunk = m_len-m_offset; + // send data + if (m_link->send(m_buf+m_offset, chunk, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + // send sequence + if (m_link->send((uint8_t *)&sequence, 1, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + // send crc + crc = calcCrc(m_buf+m_offset, chunk) + calcCrc((uint8_t *)&sequence, 1); + if (m_link->send((uint8_t *)&crc, 2, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + + if ((res=recvAck(&ack, m_dataTimeout))<0) + return res; + if (ack) + { + m_offset += chunk; + sequence++; + } + } + return CRP_RES_OK; +} + +int Chirp::sendAck(bool ack) // false=nack +{ + uint8_t c; + + if (ack) + c = CRP_ACK; + else + c = CRP_NACK; + + if (m_link->send(&c, 1, m_sendTimeout)<0) + return CRP_RES_ERROR_SEND_TIMEOUT; + + return CRP_RES_OK; +} + +int Chirp::recvHeader(uint8_t *type, ChirpProc *proc, bool wait) +{ + uint8_t c; + uint32_t chunk, startCode = 0; + uint16_t crc, rcrc; + + int return_value; + + return_value = m_link->receive(&c, 1, wait?m_headerTimeout:0); + + if (return_value < 0) { + goto chirp_recvheader__exit; + } + if (return_value == 0) { + return_value = CRP_RES_ERROR; + goto chirp_recvheader__exit; + } + + // find start code + while(1) + { + startCode >>= 8; + startCode |= (uint32_t)c<<24; + if (startCode==CRP_START_CODE) + break; + + return_value = m_link->receive(&c, 1, m_idleTimeout); + + if (return_value < 0) { + goto chirp_recvheader__exit; + } + if (return_value == 0) { + return_value = CRP_RES_ERROR; + goto chirp_recvheader__exit; + } + } + // receive rest of header + if (m_link->receive(m_buf, m_headerLen, m_idleTimeout) < 0) { + return_value = CRP_RES_ERROR_RECV_TIMEOUT; + goto chirp_recvheader__exit; + } + + if (return_value < (int) m_headerLen) { + return_value = CRP_RES_ERROR; + goto chirp_recvheader__exit; + } + + *type = *(uint8_t *)m_buf; + *proc = *(ChirpProc *)(m_buf+2); + m_len = *(uint32_t *)(m_buf+4); + crc = calcCrc(m_buf, m_headerLen); + + if (m_len>=CRP_MAX_HEADER_LEN-m_headerLen) + chunk = CRP_MAX_HEADER_LEN-m_headerLen; + else + chunk = m_len; + + return_value = m_link->receive(m_buf, chunk+2, m_idleTimeout); + + if (return_value < 0) { // +2 for crc + goto chirp_recvheader__exit; + } + if (return_value < (int) (chunk + 2)) { + return_value = CRP_RES_ERROR; + goto chirp_recvheader__exit; + } + copyAlign((char *)&rcrc, (char *)(m_buf+chunk), 2); + if (rcrc==crc+calcCrc(m_buf, chunk)) + { + m_offset = chunk; + sendAck(true); + } + else + { + sendAck(false); // send nack + return_value = CRP_RES_ERROR_CRC; + goto chirp_recvheader__exit; + } + + return_value = CRP_RES_OK; + +chirp_recvheader__exit: + + return return_value; +} + +int Chirp::recvFull(uint8_t *type, ChirpProc *proc, bool wait) +{ + int res; + uint32_t startCode; + uint32_t len, recvd; + + // receive header, with startcode check to make sure we're synced + while(1) + { + if ((res=m_link->receive(m_buf, CRP_MAX_HEADER_LEN, wait?m_headerTimeout:0))<0) + return res; + // check to see if we received less data than expected + if (resm_bufSize && (res=realloc(m_len+m_headerLen))<0) + return res; + + if (m_len+m_headerLen>recvd && !m_sharedMem) + { + len = m_len+m_headerLen; + while(recvdreceive(m_buf+recvd, len-recvd, m_idleTimeout))<0) + return res; + recvd += res; + } + } + + return CRP_RES_OK; +} + +// We assume that the probability that we send a nack and the receiver interprets a nack is 100% +// We can't assume that the probability that we send an ack and the receiver interprets it is 100% +// Scenario +// 1) we receive packet 0, redo is 0, crc is good, we increment offset, send ack. (inc) (inc) rs=0, s=0, inc +// 2) we receive packet 1, crc is bad, we send nack (!inc) (!inc) rs=1, s=1 +// 3) sender gets nack, so it resends +// 4) we receive packet 1, redo is 1, crc is good, we don't increment offset, send ack (inc) (inc) rs=1, s=1 +// 5) we receive packet 2, redo is 0, crc is bad, we send nack (!inc) (!inc) rs=2, s=2 +// 6) we receive packet 2, redo is 1, crc is good, we send ack (inc) (inc) rs=2, s=2 +// 7) we receive packet 3, redo is 0, crc is good, we send ack (inc) (inc) +// different scenario +// 1) we receive packet 0, redo is 0, crc is good, we increment offset, send ack. (inc) (inc) rs=0, s=0 +// 2) sender thinks it gets a nack, so it resends +// 3) we receive packet 0 again, but crc is bad, we send nack (!inc) (!inc) rs=1, s=0 +// 4) sender gets nack, so it resends +// 5) we receive packet 0, redo is 1, crc is good, we don't increment offset, send ack (!inc) (inc) rs=1, s=0 +// (we've essentially thrown out this packet, but that's ok, because we have a good packet 0) +// 6) we receive packet 1, redo is 0, crc is bad, we send nack (!inc) (!inc) rs=1, s=1 +// 7) we receive packet 1, redo is 1, crc is good, we send ack (inc) (inc) rs=1, s=1 +// 8) we receive packet 2, redo is 0, crc is good, we send ack (inc) (inc) rs=2, s=2 +// a redo flag is not sufficient to communicate which packet we're on because the sender can misinterpret +// any number of nacks +int Chirp::recvData() +{ + int res; + uint32_t chunk; + uint16_t crc; + uint8_t sequence, rsequence, naks; + + if (m_len+3+m_headerLen>m_bufSize && (res=realloc(m_len+3+m_headerLen))<0) // +3 to read sequence, crc + return res; + + for (rsequence=0, naks=0; m_offset=m_blkSize) + chunk = m_blkSize; + else + chunk = m_len-m_offset; + if (m_link->receive(m_buf+m_offset, chunk+3, m_dataTimeout)<0) // +3 to read sequence, crc + return CRP_RES_ERROR_RECV_TIMEOUT; + if (res<(int)chunk+3) + return CRP_RES_ERROR; + sequence = *(uint8_t *)(m_buf+m_offset+chunk); + copyAlign((char *)&crc, (char *)(m_buf+m_offset+chunk+1), 2); + if (crc==calcCrc(m_buf+m_offset, chunk+1)) + { + if (rsequence==sequence) + { + m_offset += chunk; + rsequence++; + } + sendAck(true); + naks = 0; + } + else + { + sendAck(false); + naks++; + if (naksreceive(&c, 1, timeout))<0) + return CRP_RES_ERROR_RECV_TIMEOUT; + if (res<1) + return CRP_RES_ERROR; + + if (c==CRP_ACK) + *ack = true; + else + *ack = false; + + return CRP_RES_OK; +} diff --git a/emulator/libs/Pixy/src/chirp.hpp b/emulator/libs/Pixy/src/chirp.hpp new file mode 100644 index 0000000..5edf845 --- /dev/null +++ b/emulator/libs/Pixy/src/chirp.hpp @@ -0,0 +1,291 @@ +#ifndef CHIRP_HPP +#define CHIRP_HPP + +#include +#include +#include +#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 diff --git a/emulator/libs/Pixy/src/chirpreceiver.cpp b/emulator/libs/Pixy/src/chirpreceiver.cpp new file mode 100644 index 0000000..369dd54 --- /dev/null +++ b/emulator/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); +} diff --git a/emulator/libs/Pixy/src/chirpreceiver.hpp b/emulator/libs/Pixy/src/chirpreceiver.hpp new file mode 100644 index 0000000..bcda766 --- /dev/null +++ b/emulator/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 diff --git a/emulator/libs/Pixy/src/colorlut.cpp.notneeded b/emulator/libs/Pixy/src/colorlut.cpp.notneeded new file mode 100644 index 0000000..d2f259d --- /dev/null +++ b/emulator/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 +#include +#include +#ifndef PIXY +#include "debug.h" +#endif +#include "colorlut.h" +#include "calc.h" + + + +IterPixel::IterPixel(const Frame8 &frame, const RectA ®ion) +{ + 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 (cm_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; ireset(); + while(ip->next(&uv)) + { + if (uv.m_u>sig->m_uMin) + counts[0]++; + + if (uv.m_um_uMax) + counts[1]++; + + if (uv.m_v>sig->m_vMin) + counts[2]++; + + if (uv.m_vm_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 ®ion, 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>= 9-CL_LUT_COMPONENT_SCALE; + u &= (1<>= 9-CL_LUT_COMPONENT_SCALE; + v &= (1<sig+1) + m_lut[bin] = sig+1; + } + } + } + } + } + + return 0; +} + + +void ColorLUT::clearLUT(uint8_t signum) +{ + int i; + + for (i=0; im_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 ®ion, 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; im_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)distancesize(); + 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(®ion, frame, 0)) + done |= 1<<0; + else + points->push_back(Point16(region.m_xOffset, region.m_yOffset)); + if (growRegion(®ion, frame, 1)) + done |= 1<<1; + else + points->push_back(Point16(region.m_xOffset, region.m_yOffset)); + if (growRegion(®ion, frame, 2)) + done |= 1<<2; + else + points->push_back(Point16(seed.m_x, region.m_yOffset)); + if (growRegion(®ion, 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<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 diff --git a/emulator/libs/Pixy/src/colorlut.h b/emulator/libs/Pixy/src/colorlut.h new file mode 100644 index 0000000..11cd3ac --- /dev/null +++ b/emulator/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 +#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 Points; + +class IterPixel +{ +public: + IterPixel(const Frame8 &frame, const RectA ®ion); + 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 ®ion, 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 ®ion, 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 ®ion ,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 diff --git a/emulator/libs/Pixy/src/debug.h b/emulator/libs/Pixy/src/debug.h new file mode 100644 index 0000000..337ffb3 --- /dev/null +++ b/emulator/libs/Pixy/src/debug.h @@ -0,0 +1,7 @@ +#ifndef DEBUG_H +#define DEBUG_H + +#define DBG(...) + +#endif // DEBUG_H + diff --git a/emulator/libs/Pixy/src/debuglog.h b/emulator/libs/Pixy/src/debuglog.h new file mode 100644 index 0000000..d294219 --- /dev/null +++ b/emulator/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 +#include + +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 diff --git a/emulator/libs/Pixy/src/interpreter.hpp b/emulator/libs/Pixy/src/interpreter.hpp new file mode 100644 index 0000000..ce8d196 --- /dev/null +++ b/emulator/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 diff --git a/emulator/libs/Pixy/src/link.h b/emulator/libs/Pixy/src/link.h new file mode 100644 index 0000000..3c8936d --- /dev/null +++ b/emulator/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 + +// 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 diff --git a/emulator/libs/Pixy/src/pixy.cpp b/emulator/libs/Pixy/src/pixy.cpp new file mode 100644 index 0000000..ac0f9a7 --- /dev/null +++ b/emulator/libs/Pixy/src/pixy.cpp @@ -0,0 +1,463 @@ +#include +#include +#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: + + - cmake + + Required for runtime: + + - libusb + - libboost + + \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: + + Hooking up Pixy to a Raspberry Pi + + Hooking up Pixy to a BeagleBone Black + + \section getting_help Getting Help + + Tutorials, walkthroughs, and more are available on the Pixy wiki page: + + Pixy Developer Wiki Page + + Our friendly developers and users might be able to answer your question on the forums: + + Pixy Software Discussion Forum + + Pixy Hardware Discussion Forum + +*/ + +// 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() + { + 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; + } +} diff --git a/emulator/libs/Pixy/src/pixyinterpreter.cpp b/emulator/libs/Pixy/src/pixyinterpreter.cpp new file mode 100644 index 0000000..d69cc3f --- /dev/null +++ b/emulator/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 +#include +#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(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(CCB1_data[3]); + blobs = static_cast(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(CCB2_data[5]); + B_blobs = static_cast(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(CCB2_data[3]); + A_blobs = static_cast(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; + } +} diff --git a/emulator/libs/Pixy/src/pixyinterpreter.hpp b/emulator/libs/Pixy/src/pixyinterpreter.hpp new file mode 100644 index 0000000..7144add --- /dev/null +++ b/emulator/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 +#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 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 diff --git a/emulator/libs/Pixy/src/pixytypes.h b/emulator/libs/Pixy/src/pixytypes.h new file mode 100644 index 0000000..e986bd5 --- /dev/null +++ b/emulator/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 + +#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 diff --git a/emulator/libs/Pixy/src/qqueue.cpp.notneeded b/emulator/libs/Pixy/src/qqueue.cpp.notneeded new file mode 100644 index 0000000..b40d9c6 --- /dev/null +++ b/emulator/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 +#include "qqueue.h" +#ifdef PIXY +#include +#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; idata[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; +} + + diff --git a/emulator/libs/Pixy/src/qqueue.h b/emulator/libs/Pixy/src/qqueue.h new file mode 100644 index 0000000..66a4c4d --- /dev/null +++ b/emulator/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 + +#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 diff --git a/emulator/libs/Pixy/src/simplevector.h b/emulator/libs/Pixy/src/simplevector.h new file mode 100644 index 0000000..9b252dd --- /dev/null +++ b/emulator/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 + +#define SPARE_CAPACITY 16 + +template 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(mark - epoch_).count(); +} diff --git a/emulator/libs/Pixy/src/timer.hpp b/emulator/libs/Pixy/src/timer.hpp new file mode 100644 index 0000000..53e8bce --- /dev/null +++ b/emulator/libs/Pixy/src/timer.hpp @@ -0,0 +1,40 @@ +// +// 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 __TIMER_HPP__ +#define __TIMER_HPP__ + +#include +#include +#include + +namespace util +{ + class timer + { + public: + + timer(); + + void reset(); + uint32_t elapsed(); + + private: + + boost::chrono::steady_clock::time_point epoch_; + }; +} + +#endif diff --git a/emulator/libs/Pixy/src/usblink.cpp b/emulator/libs/Pixy/src/usblink.cpp new file mode 100644 index 0000000..0a499ce --- /dev/null +++ b/emulator/libs/Pixy/src/usblink.cpp @@ -0,0 +1,175 @@ +// +// 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 +#include +#include "usblink.h" +#include "pixy.h" +#include "timer.hpp" +#include "debuglog.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 + + + +USBLink::USBLink() +{ + m_handle = 0; + m_context = 0; + m_blockSize = 64; + m_flags = LINK_FLAG_ERROR_CORRECTED; +} + +USBLink::~USBLink() +{ + if (m_handle) + libusb_close(m_handle); + if (m_context) + libusb_exit(m_context); +} + +int USBLink::open() +{ + int return_value; +#ifdef __MACOS__ + const unsigned int MILLISECONDS_TO_SLEEP = 100; +#endif + + log("pixydebug: USBLink::open()\n"); + + return_value = libusb_init(&m_context); + log("pixydebug: libusb_init() = %d\n", return_value); + + if (return_value) { + goto usblink_open__exit; + } + + m_handle = libusb_open_device_with_vid_pid(m_context, PIXY_VID, PIXY_DID); + log("pixydebug: libusb_open_device_with_vid_pid() = %d\n", m_handle); + + if (m_handle == NULL) { + return_value = PIXY_ERROR_USB_NOT_FOUND; + + goto usblink_open__exit; + } + +#ifdef __MACOS__ + return_value = libusb_reset_device(m_handle); + log("pixydebug: libusb_reset_device() = %d\n", return_value); + usleep(MILLISECONDS_TO_SLEEP * 1000); +#endif + + return_value = libusb_set_configuration(m_handle, 1); + log("pixydebug: libusb_set_configuration() = %d\n", return_value); + + if (return_value < 0) { + goto usblink_open__close_and_exit; + } + + return_value = libusb_claim_interface(m_handle, 1); + log("pixydebug: libusb_claim_interface() = %d\n", return_value); + + if (return_value < 0) { + goto usblink_open__close_and_exit; + } + +#ifdef __LINUX__ + return_value = libusb_reset_device(m_handle); + log("pixydebug: libusb_reset_device() = %d\n", return_value); +#endif + + /* Success */ + return_value = 0; + goto usblink_open__exit; + +usblink_open__close_and_exit: + /* Cleanup after error */ + + libusb_close(m_handle); + m_handle = 0; + +usblink_open__exit: + log("pixydebug: USBLink::open() returned %d\n", return_value); + + return return_value; +} + + + +int USBLink::send(const uint8_t *data, uint32_t len, uint16_t timeoutMs) +{ + int res, transferred; + + log("pixydebug: USBLink::send()\n"); + + if (timeoutMs==0) // 0 equals infinity + timeoutMs = 10; + + if ((res=libusb_bulk_transfer(m_handle, 0x02, (unsigned char *)data, len, &transferred, timeoutMs))<0) + { + log("pixydebug: libusb_bulk_transfer() = %d\n", res); +#ifdef __MACOS__ + libusb_clear_halt(m_handle, 0x02); +#endif + log("pixydebug: USBLink::send() returned %d\n", res); + return res; + } + + log("pixydebug: USBLink::send() returned %d\n", transferred); + return transferred; +} + +int USBLink::receive(uint8_t *data, uint32_t len, uint16_t timeoutMs) +{ + int res, transferred; + + log("pixydebug: USBLink::receive()\n"); + + if (timeoutMs==0) // 0 equals infinity + timeoutMs = 50; + + if ((res=libusb_bulk_transfer(m_handle, 0x82, (unsigned char *)data, len, &transferred, timeoutMs))<0) + { + log("pixydebug: libusb_bulk_transfer() = %d\n", res); +#ifdef __MACOS__ + libusb_clear_halt(m_handle, 0x82); +#endif + return res; + } + + log("pixydebug: libusb_bulk_transfer(%d bytes) = %d\n", len, res); + log("pixydebug: USBLink::receive() returned %d (bytes transferred)\n", transferred); + return transferred; +} + +void USBLink::setTimer() +{ + timer_.reset(); +} + +uint32_t USBLink::getTimer() +{ + return timer_.elapsed(); +} + diff --git a/emulator/libs/Pixy/src/usblink.h b/emulator/libs/Pixy/src/usblink.h new file mode 100644 index 0000000..fb11da1 --- /dev/null +++ b/emulator/libs/Pixy/src/usblink.h @@ -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 __USBLINK_H__ +#define __USBLINK_H__ + +#include "link.h" +#include "timer.hpp" +#include + +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(); + +private: + libusb_context *m_context; + libusb_device_handle *m_handle; + + util::timer timer_; +}; + +#endif + diff --git a/emulator/qt/ll_system.cpp b/emulator/qt/ll_system.cpp index 0bedff3..861131b 100644 --- a/emulator/qt/ll_system.cpp +++ b/emulator/qt/ll_system.cpp @@ -1,3 +1,4 @@ +#include extern "C" { #include "ll_system.h" } @@ -5,3 +6,8 @@ extern "C" { bool ll_system_init() { return true; } + +void ll_system_delay(uint32_t msec) { + QThread::msleep(msec); +} + diff --git a/emulator/qt/main.cpp b/emulator/qt/main.cpp index 7886ab5..6e5551b 100644 --- a/emulator/qt/main.cpp +++ b/emulator/qt/main.cpp @@ -17,7 +17,7 @@ void qt_init(int argc, char* argv[]) { } void app_loop() { - while(QApplication::activeWindow()!=NULL) { + while(!QApplication::closingDown()) { app_process(); QApplication::processEvents(); }