Integrated pixy into emulator. Pixy is no longer in the common/libs folder but in emulator/libs and discovery/libs
This commit is contained in:
t-moe
298
emulator/libs/Pixy/src/pixyinterpreter.cpp
Normal file
298
emulator/libs/Pixy/src/pixyinterpreter.cpp
Normal 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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user