Exemplo n.º 1
0
int write_in_queue(RT_QUEUE *msgQueue, void * data, int size) {
    void *msg;
    int err;
 
    msg = rt_queue_alloc(msgQueue, size);
    memcpy(msg, &data, size);
 
    if ((err = rt_queue_send(msgQueue, msg, sizeof (DMessage), Q_NORMAL)) < 0) {
        rt_printf("Error msg queue send: %s\n", strerror(-err));
    }
    rt_queue_free(&queueMsgGUI, msg);
 
    return err;
}
Exemplo n.º 2
0
void consumer (void *cookie)

{
    ssize_t len;
    void *msg;
    int err;

    /* Bind to a queue which has been created elsewhere, either in
       kernel or user-space. The call will block us until such queue
       is created with the expected name. The queue should have been
       created with the Q_SHARED mode set, which is implicit when
       creation takes place in user-space. */

    err = rt_queue_bind(&q_desc,"SomeQueueName",TM_INFINITE);

    if (err)
	fail();

    /* Collect each message sent to the queue by the queuer() routine,
       until the queue is eventually removed from the system by a call
       to rt_queue_delete(). */

    while ((len = rt_queue_receive(&q_desc,&msg,TM_INFINITE)) > 0)
	{
	printf("received message> len=%d bytes, ptr=%p, s=%s\n",
	       len,msg,(const char *)msg);
	rt_queue_free(&q_desc,msg);
	}

    /* We need to unbind explicitly from the queue in order to
       properly release the underlying memory mapping. Exiting the
       process unbinds all mappings automatically. */

    rt_queue_unbind(&q_desc);

    if (len != -EIDRM)
	/* We received some unexpected error notification. */
	fail();

    /* ... */
}
Exemplo n.º 3
0
void display(void *cookie)
{
	RT_QUEUE q;
	int n = 0;
	time_t start;
	struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;

	rt_queue_bind(&q, "queue");
	time(&start);
	while (!finished) {
		smpl = NULL;
		rt_queue_recv(&q, (void *)&smpl, TM_INFINITE);
		if (smpl == NULL) continue; 
		if (data_lines && (n++%data_lines) == 0) {
			time_t now, dt;
			time(&now);
			dt = now - start;
			printf("rth|%12s|%12s|%12s|%8s|     %.2ld:%.2ld:%.2ld\n", "lat min", "lat avg", "lat max", "overrun", dt/3600, (dt/60)%60, dt%60 + 1);
		}
		printf("rtd|%12ld|%12ld|%12ld|%8ld\n", smpl->minjitter, smpl->avgjitter, smpl->maxjitter, smpl->overrun);
		rt_queue_free(&q, smpl);
	}
	rt_queue_unbind(&q);
}
Exemplo n.º 4
0
/**
 *
 * @param oncillaPtr
 */
void executor(void *oncillaPtr) {

    int ret;
    unsigned long overrun;
    OncillaCmd msgBuf;

    OncillaRobot *oncilla = static_cast<OncillaRobot*> (oncillaPtr);



    sbcp::ScheduledWorkflow & w = oncilla->bus->Scheduled();
    for (int i = 0; i < 4; i++) {
        w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device, sbcp::amarsi::MotorDriver>(oncilla->legs[i]));
        oncilla->legs[i]->Motor1().GoalPosition().Set(oncilla->zeroPosRaw[2 * i]);
        oncilla->legs[i]->Motor2().GoalPosition().Set(oncilla->zeroPosRaw[2 * i + 1]);
        oncilla->servos[i]->SetCommand(oncilla->zeroPosRaw[i + 8]);

    }



    ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(oncilla->executor_task_period_ns));
    if (ret) {
        std::runtime_error ex("Xenomai task set periodic failed. Error");
        throw ex;
    }

    ret = rt_task_set_mode(0, T_PRIMARY, NULL);
    if (ret) {
        std::runtime_error ex("Xenomai task set mode failed. Error");
        throw ex;
    }

    RTIME start_time = rt_timer_read(), current_time;

    while (true) {

        rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
        //rt_mutex_release(&(oncilla->end_executor));

        ret = rt_task_wait_period(&overrun);
        if (ret) {
            std::runtime_error ex("Xenomai task wait failed. Error");
            //throw ex;
        }

        ret = rt_queue_read(&(oncilla->posQueue), &msgBuf, sizeof (msgBuf), TM_NONBLOCK);

        if (ret >= 0) {

            if (msgBuf.id == OncillaCmd::SET_POS) {

                oncilla->legs[0]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[0] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[0] / 2.0));
                oncilla->legs[0]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[1]) * oncilla->maxMotorPos[1]));
                oncilla->legs[1]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[2] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[2] / 2.0));
                oncilla->legs[1]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[3] * oncilla->maxMotorPos[3]));
                oncilla->legs[2]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[4] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[4] / 2.0));
                oncilla->legs[2]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[5]) * oncilla->maxMotorPos[5]));
                oncilla->legs[3]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[6] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[6] / 2.0));
                oncilla->legs[3]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[7] * oncilla->maxMotorPos[7]));

                for (int i = 0; i < 4; i++)
                    oncilla->servos[i]->SetCommand(msgBuf.args.doubleArgs[i + 8]);

                rt_queue_free(&(oncilla->posQueue), &msgBuf);

            } else if (msgBuf.id == OncillaCmd::RESET_TIMER) {

                if (oncilla->RT) {
                    rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);
                    oncilla->trajectQueue.clear();
                    rt_mutex_release(&(oncilla->posArrMutexRT));

                } else {
                    pthread_mutex_lock(&(oncilla->posArrMutex));
                    oncilla->trajectQueue.clear();
                    pthread_mutex_unlock(&(oncilla->posArrMutex));
                    rt_task_set_mode(0, T_PRIMARY, NULL);
                }
                
                start_time = rt_timer_read();

            }

        } else if (ret == -EINVAL || ret == -EIDRM) {
            break;
        } else if (ret != -EWOULDBLOCK) {
            std::runtime_error ex("Cannot read command from message queue. Error");
            throw ex;
        }


        try {
            w.StartTransfers();
            w.WaitForTransfersCompletion();
        } catch (sbcp::MultipleTransferError & e) {
            std::runtime_error ex("SBCP communication failed. Error");
            throw ex;
        }


        if (oncilla->RT) {

            rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);

        } else {

            // If causing timeout for real-time task try using pthread_mutex_trylock() instead
            pthread_mutex_lock(&(oncilla->posArrMutex));

        }


        for (int i = 0; i < 4; i++) {

            oncilla->currentPosRaw[2 * i] = oncilla->legs[i]->Motor1().PresentPosition().Get();
            oncilla->currentPosRaw[2 * i + 1] = oncilla->legs[i]->Motor2().PresentPosition().Get();


        }


        oncilla->currentPos[0] = (double) (oncilla->currentPosRaw[0] - oncilla->maxMotorPos[0] / 2.0) / 4096.0 * (2.0 * M_PI);
        oncilla->currentPos[1] = (1.0 - (double) oncilla->currentPosRaw[1] / (double) oncilla->maxMotorPos[1]);
        oncilla->currentPos[2] = (double) -(oncilla->currentPosRaw[2] - oncilla->maxMotorPos[2] / 2.0) / 4096.0 * (2.0 * M_PI);
        oncilla->currentPos[3] = ((double) oncilla->currentPosRaw[3] / (double) oncilla->maxMotorPos[3]);
        oncilla->currentPos[4] = (double) (oncilla->currentPosRaw[4] - oncilla->maxMotorPos[4] / 2.0) / 4096.0 * (2.0 * M_PI);
        oncilla->currentPos[5] = (1.0 - (double) oncilla->currentPosRaw[5] / (double) oncilla->maxMotorPos[5]);
        oncilla->currentPos[6] = (double) -(oncilla->currentPosRaw[6] - oncilla->maxMotorPos[6] / 2.0) / 4096.0 * (2.0 * M_PI);
        oncilla->currentPos[7] = ((double) oncilla->currentPosRaw[7] / (double) oncilla->maxMotorPos[7]);

        for (int i = 0; i < 4; i++)
            oncilla->currentPos[i + 8] = oncilla->servos[i]->Command();

        current_time = rt_timer_read();
        OncillaRobot::trajectPoint newTrajectPoint;
        newTrajectPoint.coords[0] = (double) (current_time - start_time) / 1000000;
        memcpy(newTrajectPoint.coords + 1, oncilla->currentPos, 12 * sizeof (double));

        if (oncilla->trajectQueue.size() >= oncilla->queueSize)
            oncilla->trajectQueue.pop_front();

        oncilla->trajectQueue.push_back(newTrajectPoint);

        if (oncilla->RT) {
            rt_mutex_release(&(oncilla->posArrMutexRT));

        } else {
            pthread_mutex_unlock(&(oncilla->posArrMutex));
            rt_task_set_mode(0, T_PRIMARY, NULL);
        }

        rt_mutex_release(&(oncilla->end_executor));
        //rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
    }

    /*rt_mutex_release(&(oncilla->end_executor));
    
    pthread_mutex_lock(&(oncilla->end_mutex));
    pthread_cond_signal(&(oncilla->cond));
    pthread_mutex_unlock(&(oncilla->end_mutex));
    
     */

}