void *ices_runner (void *arg)
{
struct runner *run = arg;
struct instance *current;
#ifdef HAVE_SCHED_GET_PRIORITY_MAX
int policy;
struct sched_param param;
pthread_getschedparam (pthread_self(), &policy, ¶m);
param . sched_priority = sched_get_priority_min (SCHED_OTHER);
if (pthread_setschedparam (pthread_self(), SCHED_OTHER, ¶m))
{
LOG_ERROR1 ("failed to set priority: %s", strerror (errno));
}
else
LOG_INFO0 ("set priority on runner");
#endif
LOG_INFO1 ("Runner %d ready", run->id);
while (1)
{
input_buffer *buffer;
buffer = runner_wait_for_data (run);
if (buffer == NULL)
break;
current = run->instances;
while (current != NULL)
{
add_to_stream (current, buffer);
current = current->next;
}
send_to_runner (run->next, buffer);
}
runner_close (run->next);
LOG_DEBUG1 ("Runner thread %d cleaning up streams", run->id);
current = run->instances;
while (current)
{
struct instance *next;
next = current->next;
stream_cleanup (current);
current = next;
}
close (run->fd[0]);
run->fd[0] = -1;
run->not_running = 1;
LOG_DEBUG1 ("Runner thread %d finshed", run->id);
return NULL;
}
/* process a block of data. This may be skipped, or may even kick off
* a new connection.
*
*/
static void add_to_stream (struct instance *stream, input_buffer *ib)
{
if (ib->critical)
process_critical (stream, ib);
/* LOG_DEBUG1 ("ops is %p", stream->ops); */
if (stream->ops && stream->ops->process_buffer(stream, ib) < 0)
stream_cleanup (stream);
if (ib->type == ICES_INPUT_NONE)
return;
/* the normal end of stream flush */
if (ib->eos && stream->ops)
{
if (stream->ops->flush_data)
{
LOG_DEBUG1("stream flushed due to EOS [%d]", stream->id);
stream->ops->flush_data (stream);
}
/* EOS seen and handled so disable further processing until
* another start of stream is sent. */
stream->ops = NULL;
}
return;
}
static Result *getResult(SOCKET sock, int bufsize, int retry)
{
CommDataDecoder d;
typedef CTReader Reader;
Reader *r = new Reader(sock, d, bufsize);
while (true) {
Result *result = r->readSync();
if (!result) {
if (retry <= 0) {
break;
} else {
LOG_DEBUG1(("retry"));
retry--;
#ifdef WIN32
Sleep(100000);
#else
usleep(100000);
#endif
continue;
}
}
return result;
}
return NULL;
}
void sendCalibParameters(int mouseNb)
{
uint8 code;
uint16 freq = DEFAULT_SAMPLING_FREQ;
float distEncoderMeter2 = distEncoderMeter/2.0f;
OSTimeDly(10);
SEND_CODE(MOUSE_SEND_RATIOKU);
SEND_ALL(mice[mouseNb].RatioKU);
SEND_CODE(MOUSE_SEND_RATIOKV);
SEND_ALL(mice[mouseNb].RatioKV);
SEND_CODE(MOUSE_SEND_THETA);
SEND_ALL(mice[mouseNb].theta);
SEND_CODE(MOUSE_SEND_RX);
SEND_ALL(mice[mouseNb].RX0);
SEND_CODE(MOUSE_SEND_RY);
SEND_ALL(mice[mouseNb].RY0);
SEND_CODE(MOUSE_SEND_COEFDELTA);
SEND_ALL(mice[mouseNb].Delta0);
SEND_CODE(MOUSE_SEND_VALUEVTOPS);
SEND_ALL(valueVTops);
SEND_CODE(MOUSE_SEND_DISTENCODER2);
SEND_ALL(distEncoderMeter2);
SEND_CODE(MOUSE_SEND_FREQ);
SEND_ALL(freq);
LOG_DEBUG1("code = %02X", MOUSE_SEND_FREQ);
LOG_DEBUG1("freq = %d", freq);
OSTimeDly(10);
SEND_CODE(MOUSE_RESET);
OSTimeDly(10);
SEND_CODE(MOUSE_ENABLE);
}
void runner_close (struct runner *run)
{
if (run)
{
LOG_DEBUG1 ("Runner thread %d shutting down", run->id);
close (run->fd[1]);
run->fd[1] = -1;
thread_join (run->thread);
}
}
void stream_cleanup (struct instance *stream)
{
if (stream->ops && stream->ops->flush_data)
{
LOG_DEBUG1 ("Cleanup of stream %d required", stream->id);
stream->ops->flush_data (stream);
}
output_clear (&stream->output);
}
void insert_join (Join eqn)
{//inserts an join in rep and parse trees
LOG_DEBUG1( "O: inserting join Ob" )
Ob j = get_join(eqn); Assert3(!isDepricated(j), "insert_join: bad join");
Ob l = get_lhs(eqn); Assert3(!isDepricated(l), "insert_join: bad lhs");
Ob r = get_rhs(eqn); Assert3(!isDepricated(r), "insert_join: bad rhs");
//update bool properties
j(BOOL_PROPERTIES) |= STRUCTURAL & l(BOOL_PROPERTIES)
& r(BOOL_PROPERTIES);
}
//insertion & removal
void insert_app (App eqn)
{//inserts an app in rep and parse trees
LOG_DEBUG1( "O: inserting app Ob" )
Ob app = get_app(eqn); Assert3(!isDepricated(app), "insert_app: bad app");
Ob lhs = get_lhs(eqn); Assert3(!isDepricated(lhs), "insert_app: bad lhs");
Ob rhs = get_rhs(eqn); Assert3(!isDepricated(rhs), "insert_app: bad rhs");
//update bool properties
app(BOOL_PROPERTIES) |= STRUCTURAL & lhs(BOOL_PROPERTIES)
& rhs(BOOL_PROPERTIES);
}
void insert_comp (Comp eqn)
{//inserts an comp in rep and parse trees
LOG_DEBUG1( "O: inserting comp Ob" )
Ob cmp = get_comp(eqn);Assert3(!isDepricated(cmp), "insert_comp: bad comp");
Ob lhs = get_lhs(eqn); Assert3(!isDepricated(lhs), "insert_comp: bad lhs");
Ob rhs = get_rhs(eqn); Assert3(!isDepricated(rhs), "insert_comp: bad rhs");
//update bool properties
cmp(BOOL_PROPERTIES) |= STRUCTURAL & lhs(BOOL_PROPERTIES)
& rhs(BOOL_PROPERTIES);
}
/* Implements functions to load and write Quantis registers */
quantis_register_value quantis_reg_get(quantis_pci_device* qdev,
quantis_register reg)
{
char msg[MAX_MSG_LEN];
LOG_DEBUG1("In quantis_reg_get with reg=%d\n", reg);
if (reg % 4 != 0)
{
snprintf(msg,
MAX_MSG_LEN,
"Offset (%d) in the registers array is not divisible by 4. This could crash the driver.\n",
reg);
QUANTIS_WARN(msg);
}
return (quantis_register_value)ddi_get32(qdev->regs_handle,
(quantis_register_value *)(qdev->regs + reg));
}
//interface
void merge (Ob dep, Ob rep)
{
LOG_DEBUG1( "O: merging Obs" )
Assert3(not isDepricated(rep),
"tried to merge dep Ob in to depricated rep");
//name
if (dep(NAME)) {
if (rep(NAME)) ObHdl::merge(dep(NAME), rep(NAME));
else dep(NAME)->move_to(rep);
Assert3(dep(NAME) == NULL,
"in O::merge, dep name was not forgotten");
}
//properties
rep(BOOL_PROPERTIES) |= dep(BOOL_PROPERTIES);
}
/**
* We have nothing to initialize on file opening, so we just check that the
* device really exists, and that it is opened as a character device.
*/
static int quantis_open(dev_t* dev, int flag, int otyp, cred_t* cred_p)
{
quantis_soft_state_t* soft_state;
LOG_DEBUG1("open dev %d\n", getminor(*dev));
soft_state = (quantis_soft_state_t*)ddi_get_soft_state(quantis_soft_state_p,
getminor(*dev));
if (soft_state == NULL)
{
return ENXIO;
}
if (otyp != OTYP_CHR)
{
return EINVAL;
}
return 0;
}
/**
* On cleanup, remove the minor node, unmade the register space, destroy the
* mutex and free the soft state structure.
*/
static int quantis_detach(dev_info_t *dip, ddi_detach_cmd_t cmd)
{
int instance;
quantis_soft_state_t *soft_state;
char msg[MAX_MSG_LEN];
switch (cmd)
{
case DDI_DETACH:
instance = ddi_get_instance(dip);
snprintf(msg,
MAX_MSG_LEN,
"Detaching the Quantis device %d.\n",
instance);
QUANTIS_INFO(msg);
soft_state = (quantis_soft_state_t*)ddi_get_soft_state(quantis_soft_state_p,
instance);
ddi_remove_minor_node(dip, NULL);
ddi_regs_map_free(&soft_state->regs_handle);
mutex_destroy(&soft_state->mutex);
ddi_soft_state_free(quantis_soft_state_p, instance);
mutex_enter(&quantis_mutex);
card_count--;
mutex_exit(&quantis_mutex);
return DDI_SUCCESS;
case DDI_SUSPEND:
case DDI_PM_SUSPEND:
LOG_DEBUG1("Suspending dev %d\n", instance);
return DDI_SUCCESS;
default:
return DDI_FAILURE;
}
}
void BallJoint::setAngle(double angle)
{
LOG_DEBUG1(("Joint (%s)", name()));
LOG_DEBUG1(("BallJoint::setAngle %f", angle));
}
void FixedJoint::setAngle(double angle)
{
LOG_DEBUG1(("Joint (%s)", name()));
LOG_DEBUG1(("FixedJoint::setAngle %f", angle));
}
void HingeJoint::setAngle(double angle)
{
LOG_DEBUG1(("Joint (%s)", name()));
LOG_DEBUG1(("HingeJoint::setAngle (%f, %f, %f, %f)", m_axis.x(), m_axis.y(), m_axis.z(), angle));
m_rot.setAxisAndAngle(m_axis.x(), m_axis.y(), m_axis.z(), angle);
}
/*
* log_event_deliver - event delivery thread
* Deliver all events on the event queue to syseventd.
* If the daemon can not process events, stop event
* delivery and wait for an indication from the
* daemon to resume delivery.
*
* Once all event buffers have been delivered, wait
* until there are more to deliver.
*/
static void
log_event_deliver()
{
log_eventq_t *q;
int upcall_err;
callb_cpr_t cprinfo;
CALLB_CPR_INIT(&cprinfo, &eventq_head_mutex, callb_generic_cpr,
"logevent");
/*
* eventq_head_mutex is exited (released) when there are no more
* events to process from the eventq in cv_wait().
*/
mutex_enter(&eventq_head_mutex);
for (;;) {
LOG_DEBUG1((CE_CONT, "log_event_deliver: head = %p\n",
(void *)log_eventq_head));
upcall_err = 0;
q = log_eventq_head;
while (q) {
log_eventq_t *next;
/*
* Release event queue lock during upcall to
* syseventd
*/
if (log_event_delivery == LOGEVENT_DELIVERY_HOLD) {
upcall_err = EAGAIN;
break;
}
mutex_exit(&eventq_head_mutex);
if ((upcall_err = log_event_upcall(&q->arg)) != 0) {
mutex_enter(&eventq_head_mutex);
break;
}
/*
* We may be able to add entries to
* the queue now.
*/
if (event_qfull_blocked > 0 &&
log_eventq_cnt < logevent_max_q_sz) {
mutex_enter(&event_qfull_mutex);
if (event_qfull_blocked > 0) {
cv_signal(&event_qfull_cv);
}
mutex_exit(&event_qfull_mutex);
}
mutex_enter(&eventq_head_mutex);
/*
* Daemon restart can cause entries to be moved from
* the sent queue and put back on the event queue.
* If this has occurred, replay event queue
* processing from the new queue head.
*/
if (q != log_eventq_head) {
q = log_eventq_head;
LOG_DEBUG((CE_CONT, "log_event_deliver: "
"door upcall/daemon restart race\n"));
} else {
/*
* Move the event to the sent queue when a
* successful delivery has been made.
*/
mutex_enter(&eventq_sent_mutex);
next = q->next;
q->next = log_eventq_sent;
log_eventq_sent = q;
q = next;
log_eventq_head = q;
log_eventq_cnt--;
if (q == NULL) {
ASSERT(log_eventq_cnt == 0);
log_eventq_tail = NULL;
}
mutex_exit(&eventq_sent_mutex);
}
}
switch (upcall_err) {
case 0:
/*
* Success. The queue is empty.
*/
sysevent_upcall_status = 0;
break;
case EAGAIN:
/*
* Delivery is on hold (but functional).
*/
sysevent_upcall_status = 0;
/*
* If the user has already signaled for delivery
* resumption, continue. Otherwise, we wait until
* we are signaled to continue.
*/
if (log_event_delivery == LOGEVENT_DELIVERY_CONT) {
log_event_delivery = LOGEVENT_DELIVERY_OK;
continue;
} else {
log_event_delivery = LOGEVENT_DELIVERY_HOLD;
}
LOG_DEBUG1((CE_CONT, "log_event_deliver: EAGAIN\n"));
break;
default:
LOG_DEBUG((CE_CONT, "log_event_deliver: "
"upcall err %d\n", upcall_err));
sysevent_upcall_status = upcall_err;
/*
* Signal everyone waiting that transport is down
*/
if (event_qfull_blocked > 0) {
mutex_enter(&event_qfull_mutex);
if (event_qfull_blocked > 0) {
cv_broadcast(&event_qfull_cv);
}
mutex_exit(&event_qfull_mutex);
}
break;
}
CALLB_CPR_SAFE_BEGIN(&cprinfo);
cv_wait(&log_event_cv, &eventq_head_mutex);
CALLB_CPR_SAFE_END(&cprinfo, &eventq_head_mutex);
}
/* NOTREACHED */
}
/*
* log_event_upcall - Perform the upcall to syseventd for event buffer delivery.
* Check for rebinding errors
* This buffer is reused to by the syseventd door_return
* to hold the result code
*/
static int
log_event_upcall(log_event_upcall_arg_t *arg)
{
int error;
size_t size;
sysevent_t *ev;
door_arg_t darg, save_arg;
int retry;
int neagain = 0;
int neintr = 0;
int nticks = LOG_EVENT_MIN_PAUSE;
/* Initialize door args */
ev = (sysevent_t *)&arg->buf;
size = sizeof (log_event_upcall_arg_t) + SE_PAYLOAD_SZ(ev);
darg.rbuf = (char *)arg;
darg.data_ptr = (char *)arg;
darg.rsize = size;
darg.data_size = size;
darg.desc_ptr = NULL;
darg.desc_num = 0;
if ((event_door == NULL) &&
((error = log_event_upcall_lookup()) != 0)) {
LOG_DEBUG((CE_CONT,
"log_event_upcall: event_door error (%d)\n", error));
return (error);
}
LOG_DEBUG1((CE_CONT, "log_event_upcall: 0x%llx\n",
(longlong_t)SE_SEQ((sysevent_t *)&arg->buf)));
save_arg = darg;
for (retry = 0; ; retry++) {
if ((error = door_ki_upcall_limited(event_door, &darg, NULL,
SIZE_MAX, 0)) == 0) {
break;
}
switch (error) {
case EINTR:
neintr++;
log_event_pause(2);
darg = save_arg;
break;
case EAGAIN:
/* cannot deliver upcall - process may be forking */
neagain++;
log_event_pause(nticks);
nticks <<= 1;
if (nticks > LOG_EVENT_MAX_PAUSE)
nticks = LOG_EVENT_MAX_PAUSE;
darg = save_arg;
break;
case EBADF:
LOG_DEBUG((CE_CONT, "log_event_upcall: rebinding\n"));
/* Server may have died. Try rebinding */
if ((error = log_event_upcall_lookup()) != 0) {
LOG_DEBUG((CE_CONT,
"log_event_upcall: lookup error %d\n",
error));
return (EBADF);
}
if (retry > 4) {
LOG_DEBUG((CE_CONT,
"log_event_upcall: ebadf\n"));
return (EBADF);
}
LOG_DEBUG((CE_CONT, "log_event_upcall: "
"retrying upcall after lookup\n"));
darg = save_arg;
break;
default:
cmn_err(CE_CONT,
"log_event_upcall: door_ki_upcall error %d\n",
error);
return (error);
}
}
if (neagain > 0 || neintr > 0) {
LOG_DEBUG((CE_CONT, "upcall: eagain=%d eintr=%d nticks=%d\n",
neagain, neintr, nticks));
}
LOG_DEBUG1((CE_CONT, "log_event_upcall:\n\t"
"error=%d rptr1=%p rptr2=%p dptr2=%p ret1=%x ret2=%x\n",
error, (void *)arg, (void *)darg.rbuf,
(void *)darg.data_ptr,
*((int *)(darg.rbuf)), *((int *)(darg.data_ptr))));
if (!error) {
/*
* upcall was successfully executed. Check return code.
*/
error = *((int *)(darg.rbuf));
}
return (error);
}
//add id argument by okamoto@tome(2011/8/26)
ViewImage * CTSimObj::captureView(ColorBitType cbtype, ImageDataSize size, int id)
{
NSLookup::Provider *prov = lookupProvider(Service::CAPTURE_VIEW);
if (!prov) {
LOG_ERR(("captureView : cannot get service provider info [%s:%d]", __FILE__, __LINE__));
return false;
}
SOCKET sock = prov->sock();
if (sock < 0) {
LOG_ERR(("captureView : cannot connect to service provider [%s:%d]", __FILE__, __LINE__));
return false;
}
ViewImageInfo info(IMAGE_DATA_TYPE_ANY, cbtype, size);
{
// Added id as an argument
CommRequestCaptureViewImageEncoder *enc = new CommRequestCaptureViewImageEncoder(myname(), info, id);
#if 1
// modified by sekikawa on 2010-08-26
// added error treatment in send()
// note a comment of detectEntities() in this file
int status = enc->send(sock);
delete enc;
if (status < 0)
{
prov->close();
return false;
}
#else
// orig
enc->send(sock);
delete enc;
#endif
}
ViewImage *img = 0;
CommDataDecoder d;
typedef CTReader Reader;
// modify(sekikawa)(2010/08/10)
// int retry = 10;
int retry = 500;
Reader *r = new Reader(sock, d, 100000);
while (true) {
#if 1
// sekikawa(FIX20100906)
Result *result = NULL;
try
{
result = r->readSync();
}
catch(CTReader::ConnectionClosedException &e)
{
break;
}
#else
// orig
Result *result = r->readSync();
#endif
if (!result) {
if (retry <= 0) {
// add(sekikawa)(2010/08/10)
LOG_ERR(("readSync() failed. max retry count exceeded. [%s:%d]", __FILE__, __LINE__));
break;
} else {
LOG_DEBUG1(("retrying readSync() ... [retry=%d]", retry));
retry--;
#ifdef WIN32
Sleep(100000);
#else
usleep(100000);
#endif
continue;
}
}
LOG_DEBUG1(("readSync() succeeded"));
if (result->type() == COMM_RESULT_CAPTURE_VIEW_IMAGE) {
ResultCaptureViewImageEvent *evt = (ResultCaptureViewImageEvent*)result->data();
img = evt->release();
}
delete result;
break;
}
prov->close();
delete r;
return img;
}
//added by okamoto@tome(2011/9/8)
unsigned char CTSimObj::distanceSensor(double start, double end, int id)
{
NSLookup::Provider *prov = lookupProvider(Service::DISTANCE_SENSOR);
if (!prov) {
LOG_ERR(("distanceSensor : cannot get service provider info [%s:%d]", __FILE__, __LINE__));
return false;
}
SOCKET sock = prov->sock();
if (sock < 0) {
LOG_ERR(("distanceSensor : cannot connect to service provider [%s:%d]", __FILE__, __LINE__));
return false;
}
CommRequestDistanceSensorEncoder *enc = new CommRequestDistanceSensorEncoder(myname(), start, end, id);
int status = enc->send(sock);
delete enc;
unsigned char distance;
CommDataDecoder d;
typedef CTReader Reader;
int retry = 15;
Reader *r = new Reader(sock, d, 1024);
while (true) {
Result *result = NULL;
try
{
result = r->readSync();
}
catch(CTReader::ConnectionClosedException &e)
{
break;
}
if (!result) {
if (retry <= 0) {
// add(sekikawa)(2010/08/10)
LOG_ERR(("readSync() failed. max retry count exceeded. [%s:%d]", __FILE__, __LINE__));
break;
} else {
LOG_DEBUG1(("retrying readSync() ... [retry=%d]", retry));
retry--;
#ifdef WIN32
Sleep(100000);
#else
usleep(100000);
#endif
continue;
}
}
if (result->type() == COMM_RESULT_DISTANCE_SENSOR) {
ResultDistanceSensorEvent *evt = (ResultDistanceSensorEvent*)result->data();
distance = evt->getDistance();
}
delete result;
break;
}
prov->close();
return distance;
}
//calibration function for mouse sensor
void mouse_Calibration(int lineNb, float lineDist, int turnNb)
{
int i, k;
RobotCommand cmd;
int32 DeltaUPlus[MOUSE_NUMBER];
int32 DeltaUMinus[MOUSE_NUMBER];
int32 DeltaVPlus[MOUSE_NUMBER];
int32 DeltaVMinus[MOUSE_NUMBER];
int step = 0;
mouseCalibInitStep();
//reset values of calibration
for(i=0; i<MOUSE_NUMBER; i++)
{
uint8 code;
DeltaUPlus[i] = 0;
DeltaUMinus[i] = 0;
DeltaVPlus[i] = 0;
DeltaVMinus[i] = 0;
reset(i);
//set mouse in calibration mode
code = MOUSE_CALIBRATION_MODE;
uart_SendAll(mice[i].uart, &code, sizeof(code));
}
odometrySensorUsed = MOUSE_CALIBRATION;
LOG_DEBUG("Mouse Calibration");
//make the robot run N times x meters forward and backward
for(k=0; k<lineNb; k++)
{
LOG_DEBUG1("Calibration line %d", k);
//run forward
motion_LineSpeedAcc(&cmd, lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m forward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUPlus[i] += getU(i);
DeltaVMinus[i] += getV(i);
reset(i);
}
//run backward
motion_LineSpeedAcc(&cmd, -lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m backward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUMinus[i] += getU(i);
DeltaVPlus[i] += getV(i);
reset(i);
}
}
//compute ratio for each mouse
for(i=0; i<MOUSE_NUMBER; i++)
{
float theta;
LOG_DEBUG5("Mouse %d => Du+ = %ld | Du- = %ld | Dv+ = %ld | Dv- = %ld", i, DeltaUPlus[i], DeltaUMinus[i], DeltaVPlus[i], DeltaVMinus[i]);
mice[i].RatioKU = -DeltaUMinus[i]/(float)DeltaUPlus[i];
mice[i].RatioKV = -DeltaVPlus[i]/(float)DeltaVMinus[i];
theta = atan2f( DeltaVPlus[i]-DeltaVMinus[i],
DeltaUPlus[i]-DeltaUMinus[i] );
mice[i].theta = theta;
mice[i].cosTheta = cosf(mice[i].theta);
mice[i].sinTheta = sinf(mice[i].theta);
mice[i].Delta0 = (lineNb*lineDist) / (valueVTops * ((float)DeltaUPlus[i]*cosf(theta) - (float)DeltaVMinus[i]*sinf(theta)));
LOG_DEBUG4("Ratio : coef = %f | U = %f | V = %f | Theta = %f", mice[i].Delta0, mice[i].RatioKU, mice[i].RatioKV, theta);
}
//make N * 2*Pi rotation
LOG_DEBUG("Calibration rotation");
motion_RotateSpeedAcc(&cmd, 2*M_PI*turnNb, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
LOG_DEBUG1("Calib : %d turn anticlockwise, click next when done", turnNb);
mouseCalibWaitNextStep(step++);
//compute mouse positions
for(i=0; i<MOUSE_NUMBER; i++)
{
int32 deltaU, deltaV;
int32 deltaX, deltaY;
deltaU = getU(i);
deltaV = getV(i);
reset(i);
if(deltaU < 0)
{
deltaU *= mice[i].RatioKU;
}
if(deltaV < 0)
{
deltaV *= mice[i].RatioKV;
}
deltaX = deltaU*cosf(mice[i].theta) - deltaV*sinf(mice[i].theta);
deltaY = deltaU*sinf(mice[i].theta) + deltaV*cosf(mice[i].theta);
mice[i].RX0 = deltaY / (2*M_PI*turnNb);
mice[i].RY0 = -deltaX / (2*M_PI*turnNb);
LOG_DEBUG5("Mouse %d | dU = %ld | dV = %ld | dX = %ld | dY = %ld", i, deltaU, deltaV, deltaX, deltaY);
LOG_DEBUG2("RX = %f | RY = %f", mice[i].RX0, mice[i].RY0);
sendCalibParameters(i);
}
odometrySensorUsed = MOUSE_SENSOR;
}
/**
* At attach time, we allocate the soft state structure for the current
* instance of the device.
*/
static int quantis_attach(dev_info_t *dip, ddi_attach_cmd_t cmd)
{
int instance;
quantis_soft_state_t *soft_state;
ddi_device_acc_attr_t dev_acc_attr; /* Hold the device access attributes. */
int nregs;
off_t regsize;
char msg[MAX_MSG_LEN];
LOG_DEBUG0("attach\n");
switch (cmd)
{
case DDI_ATTACH:
instance = ddi_get_instance(dip);
snprintf(msg,
MAX_MSG_LEN,
"Attaching the Quantis device %d.\n",
instance);
LOG_DEBUG0(msg);
/*
* PCI devices are self-identifying devices, so we check that we
* indeed have a Quantis QRNG card by checking that we have one
* register page with the correct size.
*/
if (ddi_dev_nregs(dip, &nregs) != DDI_SUCCESS)
{
snprintf(msg,
MAX_MSG_LEN,
"Could not get the number of register for the Quantis device %d.\n",
instance);
QUANTIS_ERROR(msg);
return DDI_FAILURE;
}
if (nregs < 4)
{
snprintf(msg,
MAX_MSG_LEN,
"The Quantis device %d has %d PCI base registers, but should have at least 4.\n",
instance,
nregs);
QUANTIS_ERROR(msg);
return DDI_FAILURE;
}
if (ddi_dev_regsize(dip, QUANTIS_REG_IDX, ®size) != DDI_SUCCESS)
{
snprintf(msg,
MAX_MSG_LEN,
"Could not get the register size for the Quantis device %d.\n",
instance);
QUANTIS_ERROR(msg);
return DDI_FAILURE;
}
if (regsize < (int)QUANTIS_REG_LENGTH)
{
snprintf(msg,
MAX_MSG_LEN,
"The size of the Quantice device (%d) registers file is %d bytes long, "
"but should be at least %u bytes long.\n",
instance,
(int)regsize,
(unsigned int)QUANTIS_REG_LENGTH);
QUANTIS_ERROR(msg);
return DDI_FAILURE;
}
LOG_DEBUG0("After test of the validity of the card, before soft state alloc.\n");
if (ddi_soft_state_zalloc(quantis_soft_state_p, instance) != DDI_SUCCESS)
{
snprintf(msg,
MAX_MSG_LEN,
"Could not allocate soft state structure for the Quantis device %d.\n",
instance);
QUANTIS_ERROR(msg);
return DDI_FAILURE;
}
soft_state = (quantis_soft_state_t *)ddi_get_soft_state(quantis_soft_state_p,
instance);
soft_state->dip = dip;
ddi_set_driver_private(dip, (caddr_t)soft_state);
soft_state->cnt = 0;
/*
* Initialize the mutex in the soft state. We have no interrupt,
* so we can set `arg' to `NULL'
*/
mutex_init(&soft_state->mutex, NULL, MUTEX_DRIVER, NULL);
if (ddi_create_minor_node(dip,
ddi_get_name(dip),
S_IFCHR,
instance,
DDI_PSEUDO,
0) == DDI_FAILURE)
{
snprintf(msg,
MAX_MSG_LEN,
"Could not create minor node for the Quantis device %d.\n",
instance);
QUANTIS_ERROR(msg);
mutex_destroy(&soft_state->mutex);
ddi_soft_state_free(quantis_soft_state_p, instance);
return DDI_FAILURE;
}
LOG_DEBUG1("ddi_get_name %s\n", ddi_get_name(dip));
dev_acc_attr.devacc_attr_version = DDI_DEVICE_ATTR_V0;
dev_acc_attr.devacc_attr_endian_flags = DDI_STRUCTURE_LE_ACC;
dev_acc_attr.devacc_attr_dataorder = DDI_STRICTORDER_ACC;
if (ddi_regs_map_setup(dip,
QUANTIS_REG_IDX,
(caddr_t *)&soft_state->regs,
0,
QUANTIS_REG_LENGTH,
&dev_acc_attr,
&soft_state->regs_handle) != DDI_SUCCESS)
{
snprintf(msg,
MAX_MSG_LEN,
"Could not map the registers space of the Quantis device %d.\n",
instance);
QUANTIS_ERROR(msg);
mutex_destroy(&soft_state->mutex);
ddi_soft_state_free(quantis_soft_state_p, instance);
return DDI_FAILURE;
}
mutex_enter(&quantis_mutex);
card_count++;
mutex_exit(&quantis_mutex);
LOG_DEBUG0("Just before mutex\n");
mutex_enter(&soft_state->mutex);
LOG_DEBUG0("Just before rng_reset.\n");
quantis_rng_reset(soft_state);
LOG_DEBUG0("Just before enable_modules.\n");
quantis_rng_enable_modules(soft_state, quantis_rng_modules_mask(soft_state));
LOG_DEBUG0("Just before release mutex.\n");
mutex_exit(&soft_state->mutex);
snprintf(msg,
MAX_MSG_LEN,
"Successfully attached the Quantis device %d. Currently, %d Quantis cards are available.\n",
instance,
card_count);
QUANTIS_INFO(msg);
# ifdef DEBUG
ddi_report_dev(dip);
# endif
return DDI_SUCCESS;
case DDI_SUSPEND:
case DDI_PM_SUSPEND:
return DDI_SUCCESS;
default:
return DDI_FAILURE;
}
}
Text * CTSimObj::getText(RawSound &rawSound)
{
NSLookup::Provider *prov = lookupProvider(Service::SOUND_RECOG);
if (!prov) {
LOG_ERR(("getText : cannot get service provider info [%s:%d]", __FILE__, __LINE__));
return false;
}
SOCKET sock = prov->sock();
if (sock < 0) {
LOG_ERR(("getText : cannot connect to service provider [%s:%d]", __FILE__, __LINE__));
return false;
}
{
CommRequestSoundRecogEncoder *enc = new CommRequestSoundRecogEncoder(rawSound);
#if 1
// modified by sekikawa on 2010-08-26
// added error treatment in send()
// note a comment of detectEntities() in this file
int status = enc->send(sock);
delete enc;
if (status < 0)
{
prov->close();
return false;
}
#else
// orig
enc->send(sock);
delete enc;
#endif
}
Text *text = 0;
CommDataDecoder d;
typedef CTReader Reader;
int retry = 10;
Reader *r = new Reader(sock, d, 1024);
while (true) {
#if 1
// sekikawa(FIX20100906)
Result *result = NULL;
try
{
result = r->readSync();
}
catch(CTReader::ConnectionClosedException &e)
{
break;
}
#else
// orig
Result *result = r->readSync();
#endif
if (!result) {
if (retry <= 0) {
break;
} else {
LOG_DEBUG1(("retry"));
retry--;
#ifdef WIN32
Sleep(100000);
#else
usleep(100000);
#endif
continue;
}
}
if (result->type() == COMM_RESULT_SOUND_RECOG) {
ResultSoundRecogEvent *evt = (ResultSoundRecogEvent*)result->data();
text = evt->releaseText();
}
delete result;
break;
}
prov->close();
delete r;
return text;
}
/**
* In the same way, we have no resources to get rid off when a file is closed,
* so just return a success.
*/
static int quantis_close(dev_t dev, int flag, int otyp, cred_t* cred_p)
{
LOG_DEBUG1("close dev %d\n", getminor(dev));
return 0;
}
// Distance sensor more than 1D
ViewImage *CTSimObj::distanceSensorD(double start, double end, int id, bool map)
{
NSLookup::Provider *prov = lookupProvider(Service::DISTANCE_SENSOR);
if (!prov) {
LOG_ERR(("distanceSensor1D : cannot get service provider info [%s:%d]", __FILE__, __LINE__));
return false;
}
SOCKET sock = prov->sock();
if (sock < 0) {
LOG_ERR(("distanceSensor1D : cannot connect to service provider [%s:%d]", __FILE__, __LINE__));
return false;
}
CommRequestDistanceSensorEncoder *enc;
if(map == false)
{
enc = new CommRequestDistanceSensorEncoder(myname(), start, end, id, 1);
}
else
{
// Encoder for 2D distance sensor
enc = new CommRequestDistanceSensorEncoder(myname(), start, end, id, 2);
}
// CommRequestDistanceSensorEncoder enc(myname(), id);
int status = enc->send(sock);
delete enc;
ViewImageInfo info(IMAGE_DATA_TYPE_ANY, DEPTHBIT_8, IMAGE_320X1);
ViewImage *img;
CommDataDecoder d;
typedef CTReader Reader;
int retry;
if(map == true)
retry = 25;
else
retry = 20;
Reader *r = new Reader(sock, d, 100000);
while (true) {
Result *result = NULL;
try
{
result = r->readSync();
}
catch(CTReader::ConnectionClosedException &e)
{
break;
}
if (!result) {
if (retry <= 0) {
// add(sekikawa)(2010/08/10)
LOG_ERR(("readSync() failed. max retry count exceeded. [%s:%d]", __FILE__, __LINE__));
break;
} else {
LOG_DEBUG1(("retrying readSync() ... [retry=%d]", retry));
retry--;
#ifdef WIN32
Sleep(100000);
#else
usleep(100000);
#endif
continue;
}
}
if (result->type() == COMM_RESULT_CAPTURE_VIEW_IMAGE) {
ResultCaptureViewImageEvent *evt = (ResultCaptureViewImageEvent*)result->data();
img = evt->release();
}
/*
if (result->type() == COMM_RESULT_DISTANCE_SENSOR1D) {
ResultDistanceSensor1DEvent *evt = (ResultDistanceSensor1DEvent*)result->data();
distance = evt->getDistance();
}
*/
delete result;
break;
}
prov->close();
return img;
}
void *metadata_thread_signal(void *arg)
{
char buf[1024];
input_module_t *mod = arg;
while(1)
{
char **md = NULL;
int comments = 0;
FILE *file;
while(metadata_update_signalled == 0)
{
thread_cond_wait(&ices_config->event_pending_cond);
if (ices_config->shutdown)
{
LOG_INFO0 ("metadata thread shutting down");
return NULL;
}
LOG_DEBUG0("meta thread wakeup");
}
metadata_update_signalled = 0;
file = fopen(ices_config->metadata_filename, "r");
if(!file) {
LOG_WARN2("Failed to open file \"%s\" for metadata update: %s",
ices_config->metadata_filename, strerror(errno));
continue;
}
LOG_DEBUG1("reading metadata from \"%s\"", ices_config->metadata_filename);
while(fgets(buf, 1024, file))
{
if(buf[0] == '\n')
break;
else
{
if(buf[strlen(buf)-1] == '\n')
buf[strlen(buf)-1] = 0;
md = realloc(md, (comments+2)*sizeof(char *));
md[comments] = malloc(strlen(buf)+1);
memcpy(md[comments], buf, strlen(buf)+1);
comments++;
LOG_INFO2 ("tag %d is %s", comments, buf);
}
}
fclose(file);
if(md) /* Don't update if there's nothing there */
{
md[comments]=0;
/* Now, let's actually use the new data */
LOG_INFO0("Updating metadata");
mod->handle_event(mod,EVENT_METADATAUPDATE,md);
}
else
LOG_INFO0("No metadata has been read");
}
}
/*
* log_event_upcall - Perform the upcall to syseventd for event buffer delivery.
* Check for rebinding errors
* This buffer is reused to by the syseventd door_return
* to hold the result code
*/
static int
log_event_upcall(log_event_upcall_arg_t *arg)
{
int error;
size_t size;
sysevent_t *ev;
door_arg_t darg, save_arg;
int retry;
int neagain = 0;
int neintr = 0;
int nticks = LOG_EVENT_MIN_PAUSE;
/* Initialize door args */
ev = (sysevent_t *)&arg->buf;
size = sizeof (log_event_upcall_arg_t) + SE_PAYLOAD_SZ(ev);
darg.rbuf = (char *)arg;
darg.data_ptr = (char *)arg;
darg.rsize = size;
darg.data_size = size;
darg.desc_ptr = NULL;
darg.desc_num = 0;
LOG_DEBUG1((CE_CONT, "log_event_upcall: 0x%llx\n",
(longlong_t)SE_SEQ((sysevent_t *)&arg->buf)));
save_arg = darg;
for (retry = 0; ; retry++) {
mutex_enter(&event_door_mutex);
if (event_door == NULL) {
mutex_exit(&event_door_mutex);
return (EBADF);
}
if ((error = door_ki_upcall_limited(event_door, &darg, NULL,
SIZE_MAX, 0)) == 0) {
mutex_exit(&event_door_mutex);
break;
}
/*
* EBADF is handled outside the switch below because we need to
* hold event_door_mutex a bit longer
*/
if (error == EBADF) {
/* Server died */
door_ki_rele(event_door);
event_door = NULL;
mutex_exit(&event_door_mutex);
return (error);
}
mutex_exit(&event_door_mutex);
/*
* The EBADF case is already handled above with event_door_mutex
* held
*/
switch (error) {
case EINTR:
neintr++;
log_event_pause(2);
darg = save_arg;
break;
case EAGAIN:
/* cannot deliver upcall - process may be forking */
neagain++;
log_event_pause(nticks);
nticks <<= 1;
if (nticks > LOG_EVENT_MAX_PAUSE)
nticks = LOG_EVENT_MAX_PAUSE;
darg = save_arg;
break;
default:
cmn_err(CE_CONT,
"log_event_upcall: door_ki_upcall error %d\n",
error);
return (error);
}
}
if (neagain > 0 || neintr > 0) {
LOG_DEBUG((CE_CONT, "upcall: eagain=%d eintr=%d nticks=%d\n",
neagain, neintr, nticks));
}
LOG_DEBUG1((CE_CONT, "log_event_upcall:\n\t"
"error=%d rptr1=%p rptr2=%p dptr2=%p ret1=%x ret2=%x\n",
error, (void *)arg, (void *)darg.rbuf,
(void *)darg.data_ptr,
*((int *)(darg.rbuf)), *((int *)(darg.data_ptr))));
if (!error) {
/*
* upcall was successfully executed. Check return code.
*/
error = *((int *)(darg.rbuf));
}
return (error);
}