int
EpuckDriver::MainSetup()
{
if(this->serialPort->initialize() == -1)
{
PLAYER_ERROR1("%s",this->serialPort->getError().c_str());
return -1;
}
// Request e-puck side program version
this->serialPort->sendInt(0x01);
if(this->serialPort->recvUnsigned() != this->EXPECTED_EPUCK_SIDE_VERSION)
{
PLAYER_ERROR("The e-puck side program version isn't the expected");
return -1;
}
if(epuckCamera.get() != NULL)
{
try
{
this->epuckCamera->Initialize();
std::string version = this->epuckCamera->GetCameraVersion();
PLAYER_MSG1(1,"E-puck camera initialized. Camera Version: %s", version.c_str());
}
catch(std::exception &e)
{
PLAYER_ERROR1("%s", e.what());
return -1;
}
}
return 0;
}
int
player_sd_unregister(player_sd_t* sd,
const char* name)
{
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
player_sd_mdns_dev_t* dev = NULL;
int i;
for(i=0;i<mdns->mdnsDevs_len;i++)
{
if(mdns->mdnsDevs[i].valid && !strcmp(name,mdns->mdnsDevs[i].sdDev.name))
{
dev = mdns->mdnsDevs + i;
break;
}
}
if(dev)
{
DNSServiceRefDeallocate(dev->regRef);
dev->valid = 0;
PLAYER_MSG1(2,"Unregistration of %s successful", name);
return(0);
}
else
{
PLAYER_ERROR1("Failed to find and unregister device %s", name);
return(-1);
}
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int CameraV4L2::Setup()
{
int err=0;
int i;
source = back_source;
norm = back_norm;
depth = back_depth;
width = back_width;
height = back_height;
fieldType = back_fieldType;
v4l2_type_id = back_v4l2_type_id;
PLAYER_MSG1(0,"Camerav4l2: Setting up device %s", this->device);
this->fg = fg2_createFrameGrabber();
// set initial settings
if (fg2_open(this->fg, this->device)!=0)
{
PLAYER_ERROR1("unable to open %s", this->device);
fg2_delete(&fg);
return -1;
}
PLAYER_MSG0(1,"Camerav4l2: device opened, aplying settings");
err |= fg2_set_source(this->fg, this->source, this->sourceCh);
err |= fg2_set_source_norm(this->fg, this->norm);
if (err!=0){
PLAYER_ERROR("Camerav4l2: failed source or norm");
fg2_delete(&fg);
return -1;
}
for(i=0; i<numOfCtls; i++){
fg2_setControlValue(fg, ctlNames[i], ctlVals[i]);
}
if (fg2_setPixelSettings(fg, this->width, this->height, this->v4l2_type_id, this->fieldType, this->depth)==0) {
} else {
PLAYER_ERROR("Camerav4l2: nie udalo sie ustawic podanych parametrow");
//fg2_delete(&fg);
//return -1;
}
// finally start streaming with new settings
if (fg2_startCapture(fg)!=0){
fg2_delete(&fg);
return -1;
}
// Start the driver thread.
this->StartThread();
return 0;
}
////////////////////////////////////////////////////////////////////////////////
/// Process incoming requests
int CameraV4L2::HandleRequests()
{
void *client;
char request[PLAYER_MAX_REQREP_SIZE];
char outBuf[PLAYER_MAX_REQREP_SIZE];
int len;
while ((len = GetConfig(&client, &request, sizeof(request),NULL)) > 0)
{
int ret = -1;
PLAYER_MSG2(2,"Got Reguest %c (size: %d)", request[0], len);
if (len>1 && (request[0]=='g' || request[0]=='G')){
// pobranie jakiejs wartosci
PLAYER_MSG0(2,"Get type request");
ret = handleCommand(request, len, outBuf, PLAYER_MAX_REQREP_SIZE-1);
if (ret==0) {
outBuf[PLAYER_MAX_REQREP_SIZE-1] = '\0';
PLAYER_MSG1(2,"Sending Ack: %s", outBuf);
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, outBuf, strlen(outBuf)+1,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sendinf NACK");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
} else {
if (len>0) {
ret = handleCommand(request, len);
}
if (ret == 0){
PLAYER_MSG0(2,"Sending Ack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sending Nack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
}
// if framegrabber is slow(1-10s), it is worth handling all commands at once
//usleep(30000);
}
return 0;
}
int
player_sd_register(player_sd_t* sd,
const char* name,
player_devaddr_t addr)
{
DNSServiceErrorType sdErr;
char recordval[PLAYER_SD_TXT_MAXLEN];
int i,j;
player_sd_mdns_t* mdns = (player_sd_mdns_t*)(sd->sdRef);
player_sd_mdns_dev_t* dev;
char nameBuf[PLAYER_SD_NAME_MAXLEN];
// Find a spot for this device
for(i=0;i<mdns->mdnsDevs_len;i++)
{
if(!mdns->mdnsDevs[i].valid)
break;
}
if(i==mdns->mdnsDevs_len)
{
// Make the list bigger
if(!mdns->mdnsDevs_len)
mdns->mdnsDevs_len = PLAYER_SD_MDNS_DEVS_LEN_INITIAL;
else
mdns->mdnsDevs_len *= PLAYER_SD_MDNS_DEVS_LEN_MULTIPLIER;
mdns->mdnsDevs =
(player_sd_mdns_dev_t*)realloc(mdns->mdnsDevs,
(mdns->mdnsDevs_len *
sizeof(player_sd_mdns_dev_t)));
assert(mdns->mdnsDevs);
for(j=i;j<mdns->mdnsDevs_len;j++)
mdns->mdnsDevs[j].valid = 0;
}
dev = mdns->mdnsDevs + i;
dev->fail = 0;
memset(dev->sdDev.name,0,sizeof(dev->sdDev.name));
strncpy(dev->sdDev.name,name,sizeof(dev->sdDev.name)-1);
memset(dev->sdDev.hostname,0,sizeof(dev->sdDev.hostname));
packedaddr_to_dottedip(dev->sdDev.hostname,sizeof(dev->sdDev.hostname),
addr.host);
dev->sdDev.robot = addr.robot;
dev->sdDev.interf = addr.interf;
dev->sdDev.index = addr.index;
dev->nameIdx = 1;
TXTRecordCreate(&(dev->txtRecord),sizeof(dev->txtBuf),dev->txtBuf);
memset(recordval,0,sizeof(recordval));
snprintf(recordval, sizeof(recordval), "%s:%u",
interf_to_str(addr.interf), addr.index);
if((sdErr = TXTRecordSetValue(&(dev->txtRecord),
"device",
strlen(recordval),
recordval)))
{
PLAYER_ERROR1("TXTRecordSetValue returned error: %d", sdErr);
return(-1);
}
memset(nameBuf,0,sizeof(nameBuf));
strncpy(nameBuf,name,sizeof(nameBuf)-1);
sdErr = kDNSServiceErr_NameConflict;
// Avahi can return the kDNSServiceErr_NameConflict immediately.
while(sdErr == kDNSServiceErr_NameConflict)
{
sdErr = DNSServiceRegister(&(dev->regRef),
0,
0,
nameBuf,
PLAYER_SD_SERVICENAME,
NULL,
NULL,
addr.robot,
TXTRecordGetLength(&(dev->txtRecord)),
TXTRecordGetBytesPtr(&(dev->txtRecord)),
registerCB,
(void*)dev);
if(sdErr == kDNSServiceErr_NameConflict)
{
// Pick a new name
memset(nameBuf,0,sizeof(nameBuf));
snprintf(nameBuf,sizeof(nameBuf),"%s (%d)",
name,dev->nameIdx++);
}
}
if(sdErr != kDNSServiceErr_NoError)
{
PLAYER_ERROR1("DNSServiceRegister returned error: %d", sdErr);
return(-1);
}
else
{
dev->valid = 1;
if(strcmp(nameBuf,name))
PLAYER_WARN2("Changing service name of %s to %s\n",
name,nameBuf);
PLAYER_MSG1(2,"Registration of %s successful", name);
return(0);
}
}
/**
* handle command without reply
* @param cmd command in text representation
* @param len length of cmd (including '\0')
* @return 0 ok, -1 failure
*/
int CameraV4L2::handleCommand(char *cmd, int len){
int w;
int h;
enum v4l2_field fld;
int ret=-1;
unsigned int a,b,c; // backup
double dblVal;
ret = -1; // error on default
PLAYER_MSG1(2,"Got command %s", cmd);
switch(cmd[0]){
case 's':
case 'S':
if (sscanf(cmd+1," %d %d", &w, &h)!=2)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, w, h, this->v4l2_type_id, fieldType);
fg2_startCapture(fg);
break;
case 'i':
case 'I':
// todo: rollback if needed
if (cmd[2]>='0' && cmd[2]<='9')
sscanf(cmd+2,"%d", &(this->source));
else {
this->source = -1;
}
fg2_stopCapture(fg);
ret = fg2_set_source(this->fg, this->source, cmd+2);
fg2_startCapture(fg);
break;
case 'n':
case 'N':
a = this->norm;
b = this->width;
c = this->height;
if (cmd[1]!=' ' || selectFormat(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = fg2_set_source_norm(this->fg, this->norm);
ret |= tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
if (ret!=0) {
this->norm = a;
this->width = b;
this->height = c;
// known working?
fg2_set_source_norm(this->fg, this->norm);
tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
}
fg2_startCapture(fg);
break;
case 'm':
case 'M':
a = this->data.format;
b = this->depth;
c = this->v4l2_type_id;
if (cmd[1]!=' ' || selectFormat(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
if (ret!=0) {
this->data.format = a;
this->depth = b;
this->v4l2_type_id = c;
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
}
fg2_startCapture(fg);
break;
case 'w':
case 'W':
// todo: rollback if needed
if (cmd[2]>='0' && cmd[2]<='9'){
sscanf(cmd+2,"%d", &(this->flip_rb));
ret = 0;
}
break;
case 'f':
case 'F':
fld = fieldType;
if (cmd[1]!=' ' || selectField(cmd+2)!=0)
break;
fg2_stopCapture(fg);
ret = tryPixelSettings(fg, this->width, this->height, this->v4l2_type_id, fieldType);
fg2_startCapture(fg);
if (ret!=0) {
fieldType = fld;
}
break;
case 'c':
case 'C':
if (cmd[1] == 'i' || cmd[1] == 'I') {
if (sscanf(cmd+2, "%lf %u", &dblVal, &a)!=2)
break;
ret = fg2_setControlValueI(fg, a, dblVal);
break;
}
for(w=2; w<len-1; w++)
if (cmd[w]==' ' || cmd[w+1]=='\0')
break;
if (sscanf(cmd+2,"%lf",&dblVal) != 1)
break;
ret = fg2_setControlValue(fg, cmd+w+1, dblVal);
break;
default:
PLAYER_WARN1("Unknown command %s",cmd);
break;
}
return ret;
}
/**
* handle command and generate reply
* @param cmd command in text representation
* @param len length of cmd (including '\0')
* @param reply buffer for reply
* @param rlen length of buffer for reply
* @return 0 ok, -1 failure
*/
int CameraV4L2::handleCommand(char *cmd, int len, char *reply, int rlen){
//enum v4l2_field fld;
int ret=-1;
unsigned int a; // backup
double dblVal;
ret = -1; // error on default
PLAYER_MSG1(2,"Got command get type %s", cmd);
switch(cmd[1]){
case 'c': // controls
case 'C':
if (len<3)
break;
if (cmd[2] == '\0'){
// count controls
PLAYER_MSG0(2,"counting controls");
a = fg2_countControls(fg);
ret = 0;
snprintf(reply, rlen, "%d", a);
break;
}
if (len<4)
break;
if (cmd[2] == 'i' || cmd[2] == 'I') {
if (sscanf(cmd+3, "%u", &a)!=1)
break;
dblVal = fg2_getControlValueI(fg, a);
ret = 0;
snprintf(reply, rlen, "%f", dblVal);
break;
}
if (cmd[2] == 'n' || cmd[2] == 'N' && cmd[3]==' '){
if (sscanf(cmd+3, "%u", &a)!=1)
break;
// get name
const char *name = fg2_getControlName(fg, a);
if (name==0)
break;
ret = 0;
snprintf(reply, rlen, "%s", name);
break;
}
if (cmd[2] != ' ')
break;
dblVal = fg2_getControlValue(fg, cmd + 3);
ret = 0;
snprintf(reply, rlen, "%f", dblVal);
break;
case 'f': // fields
case 'F':
{
int i=0;
for(i=0; fieldy[i].name; i++)
if (fieldy[i].fieldType == this->fieldType)
break;
if (fieldy[i].name) {
snprintf(reply, rlen, "%s", fieldy[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown field type: %d", this->fieldType);
}
break;
}
case 'm': // modes
case 'M':
{
int i=0;
for(i=0; formaty[i].name; i++)
if (formaty[i].v4l2_type_id == this->v4l2_type_id)
break;
if (formaty[i].name) {
snprintf(reply, rlen, "%s", formaty[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown pixel format type: %d", this->v4l2_type_id);
}
break;
}
case 'n': // norms
case 'N':
{
int i=0;
for(i=0; normy[i].name; i++)
if (normy[i].id == this->norm)
break;
if (formaty[i].name) {
snprintf(reply, rlen, "%s", normy[i].name);
ret = 0;
} else {
PLAYER_WARN1("Unknown pixel format type: %d", this->norm);
}
break;
}
case 'i': // source name
case 'I':
{
ret = 0;
snprintf(reply, rlen, "%s", fg2_get_source_name(fg));
break;
}
case '\0': // driver name
{
ret = 0;
snprintf(reply, rlen, "%s", fg2_get_device_name(fg));
break;
}
default:
PLAYER_ERROR1("Unknown command2 %s",cmd);
break;
}
return ret;
}
