aaron/discoverpixy
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Integrated pixy into emulator. Pixy is no longer in the common/libs folder but in emulator/libs and discovery/libs

Browse Source
This commit is contained in:
t-moe
2015-04-03 21:42:46 +02:00
parent c570bda350
commit cab86098c5
68 changed files with 6968 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
discovery/libs/Pixy/.gitignore vendored Normal file
View File

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

59
discovery/libs/Pixy/Makefile Normal file
View File

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

150
discovery/libs/Pixy/hello_pixy.cpp Normal file
View File

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

549
discovery/libs/Pixy/src/blob.cpp.notneeded Normal file
View File

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

357
discovery/libs/Pixy/src/blob.h Normal file
View File

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

1084
discovery/libs/Pixy/src/blobs.cpp.notneeded Normal file
View File

File diff suppressed because it is too large Load Diff

111
discovery/libs/Pixy/src/blobs.h Normal file
View File

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

108
discovery/libs/Pixy/src/calc.cpp.notneeded Normal file
View File

@@ -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);
}

35
discovery/libs/Pixy/src/calc.h Normal file
View File

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

1384
discovery/libs/Pixy/src/chirp.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

291
discovery/libs/Pixy/src/chirp.hpp Normal file
View File

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

38
discovery/libs/Pixy/src/chirpreceiver.cpp Normal file
View File

@@ -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);
}

43
discovery/libs/Pixy/src/chirpreceiver.hpp Normal file
View File

@@ -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

642
discovery/libs/Pixy/src/colorlut.cpp.notneeded Normal file
View File

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

129
discovery/libs/Pixy/src/colorlut.h Normal file
View File

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

7
discovery/libs/Pixy/src/debug.h Normal file
View File

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

36
discovery/libs/Pixy/src/debuglog.h Normal file
View File

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

26
discovery/libs/Pixy/src/interpreter.hpp Normal file
View File

@@ -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

76
discovery/libs/Pixy/src/link.h Normal file
View File

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

463
discovery/libs/Pixy/src/pixy.cpp Normal file
View File

@@ -0,0 +1,463 @@
#include <stdio.h>
#include <string.h>
#include "pixy.h"
#include "pixydefs.h"
#include "pixyinterpreter.hpp"
PixyInterpreter interpreter;
/**
\mainpage libpixyusb-0.4 API Reference
\section introduction Introduction
libpixyusb is an open source library that allows you to communicate with
Pixy over the USB protocol.
This documentation is aimed at application developers wishing to send
commands to Pixy or read sensor data from Pixy.
\section library_features Library features
- Read blocks with or without color codes
- RGB LED control (color/intensity)
- Auto white balance control
- Auto exposure compensation control
- Brightness control
- Servo position control/query
- Custom commands
\section dependencies Dependencies
Required to build:
- <a href=http://www.cmake.org>cmake</a>
Required for runtime:
- <a href=http://www.libusb.org>libusb</a>
- <a href=http://www.boost.org>libboost</a>
\section getting_started Getting Started
The libpixyusb API reference documentation can be found here:
libpixyusb API Reference
Some tutorials that use libpixyusb can be found here:
<a href=http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Raspberry_Pi>Hooking up Pixy to a Raspberry Pi</a>
<a href=http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Beaglebone_Black>Hooking up Pixy to a BeagleBone Black</a>
\section getting_help Getting Help
Tutorials, walkthroughs, and more are available on the Pixy wiki page:
<a href=http://www.cmucam.org/projects/cmucam5/wiki>Pixy Developer Wiki Page</a>
Our friendly developers and users might be able to answer your question on the forums:
<a href=http://www.cmucam.org/projects/cmucam5/boards/9>Pixy Software Discussion Forum</a>
<a href=http://www.cmucam.org/projects/cmucam5/boards/8>Pixy Hardware Discussion Forum</a>
*/
// Pixy C API //
extern "C"
{
static struct
{
int error;
const char * text;
} PIXY_ERROR_TABLE[] = {
{ 0, "Success" },
//{ PIXY_ERROR_USB_IO, "USB Error: I/O" },
//{ PIXY_ERROR_USB_BUSY, "USB Error: Busy" },
//{ PIXY_ERROR_USB_NO_DEVICE, "USB Error: No device" },
//{ PIXY_ERROR_USB_NOT_FOUND, "USB Error: Target not found" },
{ PIXY_ERROR_CHIRP, "Chirp Protocol Error" },
{ PIXY_ERROR_INVALID_COMMAND, "Pixy Error: Invalid command" },
{ 0, 0 }
};
static int pixy_initialized = false;
int pixy_init()
{
int return_value;
return_value = interpreter.init();
if(return_value == 0)
{
pixy_initialized = true;
}
return return_value;
}
int pixy_get_blocks(uint16_t max_blocks, struct Block * blocks)
{
return interpreter.get_blocks(max_blocks, blocks);
}
int pixy_blocks_are_new()
{
return interpreter.blocks_are_new();
}
int pixy_service()
{
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;
}
}

298
discovery/libs/Pixy/src/pixyinterpreter.cpp Normal file
View File

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

149
discovery/libs/Pixy/src/pixyinterpreter.hpp Normal file
View File

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

271
discovery/libs/Pixy/src/pixytypes.h Normal file
View File

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

103
discovery/libs/Pixy/src/qqueue.cpp.notneeded Normal file
View File

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

105
discovery/libs/Pixy/src/qqueue.h Normal file
View File

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

95
discovery/libs/Pixy/src/simplevector.h Normal file
View File

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

52
discovery/libs/Pixy/src/usblink.cpp Normal file
View File

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

20
discovery/libs/Pixy/src/usblink.h Normal file
View File

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