robomon_orte.h

//
// C++ Interface: robomon_orte
//
// Description: 
//
//
// Author: Martin Zidek,,, <martin@martin-laptop>, (C) 2007
//
// Copyright: See COPYING file that comes with this distribution
//
//
#include <orte.h>
#ifdef CONFIG_ORTE_RTL
  #include <linux/module.h>
  #include <posix/pthread.h>
  #define printf rtl_printf
#elif defined CONFIG_ORTE_RTAI
  #include <linux/module.h>
  #include <rtai/compat.h>  
  #define printf rt_printk
#else
  #include <stdio.h>
#endif

#include <RobotType.h>
#include <RobotTypeArr.h>

ORTEDomain *d;
ORTEPublication *publisherDO;
ORTEPublication *publisherMotor;
ORTEPublication *publisherServa;
ORTEPublication *publisherSharpShort;
ORTEPublication *publisherSharpLong;



unsigned int accelRecv[3];
stateDigIn stateDI;
uint16_t stateIR;
unsigned char stateIR2;
bool stateDO[8];
short int motorSpeedCmd[2];
uint16_t sharpToRecv[4];
//uint8_t serva[8];

motionStatus ms;
motionSpeed mspeed;
stateInnerIR sir;
sharpLongs ss1;
sharpShorts ss2;
stateServa serva;
position pozice;

void recvPositionCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvSharp1CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvSharp2CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvDICallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvIRCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvAccelCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void sendCallBackServo(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam);
void recvPosCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);
void recvMsCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam);

void *publisherCreateSensLong(void *arg) {      //FIXME
        NtpTime             persistence, delay;
        robomon* ptr = (robomon*)arg;

        sharpLongs_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        publisherSharpLong=ORTEPublicationCreate(d, "sharpSouper", "sharpLongs", &(ptr->sharpySouper), &persistence, 2, NULL, arg, &delay);
        return arg;
}

void *publisherCreateSensShort(void *arg) {     //FIXME
        NtpTime             persistence, delay;
        robomon* ptr = (robomon*)arg;
        
        sharpShorts_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        publisherSharpShort=ORTEPublicationCreate(d, "sharpLahve", "sharpShorts", &(ptr->sharpyLahve), &persistence, 2, NULL, arg, &delay);
        return arg;
}

void *publisherCreateServo(void *arg) {
        NtpTime             persistence, delay;

        stateServa_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        publisherServa=ORTEPublicationCreate(d, "serva", "stateServa", &serva, &persistence, 1, NULL, arg, &delay);
        return arg;
}

void *publisherDOCreate(void *arg) {
        NtpTime             persistence, delay;

        ORTETypeRegisterAdd(d,"DOMsg",NULL,NULL,NULL,sizeof(stateDO));
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        publisherDO=ORTEPublicationCreate(d, "DO", "DOMsg", &stateDO, &persistence, 1, NULL,  NULL, &delay);
        return arg;
}

void *publisherMotorCreate(void *arg) {
        NtpTime             persistence, delay;

        motionSpeed_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        publisherMotor=ORTEPublicationCreate(d, "motor", "motionSpeed", &mspeed, &persistence, 1, NULL,  NULL, &delay);
        return arg;
}

void *subscriberPositionCreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        position_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "pos", "position", &pozice, &deadline, &minimumSeparation, recvPosCallBack, arg, IPADDRESS_INVALID);
        return arg;
}


void *subscriberSharp1Create(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        sharpLongs_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpSouper", "sharpLongs", &ss1, //FIXME
        &deadline, &minimumSeparation, recvSharp1CallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void * subscriberSharp2Create(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;
        
        sharpShorts_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpLahve", "sharpShorts", &ss2, &deadline, &minimumSeparation, recvSharp2CallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberCreateDI(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        stateDigIn_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "DI", "stateDigIn", &stateDI, &deadline, &minimumSeparation, recvDICallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberCreateIR(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        stateInnerIR_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "IR", "stateInnerIR", &sir, &deadline, &minimumSeparation, recvIRCallBack, arg, IPADDRESS_INVALID);
        return arg;
}


void *subscriberCreateMotionStatus(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        motionStatus_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "motion_status", "motionStatus", &ms, &deadline, &minimumSeparation, recvMsCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberAccelCreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        ORTETypeRegisterAdd(d,"AccelMsg",NULL,NULL,NULL,sizeof(accelRecv));
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "accel", "AccelMsg", &accelRecv, &deadline, &minimumSeparation, recvAccelCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void recvPosCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        robomon* ptr = (robomon*)recvCallBackParam;
        QEvent* event = new QEvent(EVENT_POS);
  switch (info->status) {
    case NEW_DATA:
        QCoreApplication::postEvent(ptr, event);        
        break;
    case DEADLINE:
      printf("pos deadline occurred\n");
      break;
  }
}

void recvSharp1CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        robomon* ptr = (robomon*)recvCallBackParam;
        QEvent* event = new QEvent(EVENT_SHARP);
  switch (info->status) {
    case NEW_DATA:
        QCoreApplication::postEvent(ptr, event);
        break;
    case DEADLINE:
      printf("sharp1 deadline occurred\n");
      break;
  }
}

void recvSharp2CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        robomon* ptr = (robomon*)recvCallBackParam;
        QEvent* event = new QEvent(EVENT_SHARP);
        switch (info->status) {
                case NEW_DATA:
                        QCoreApplication::postEvent(ptr, event);        
                        break;
                case DEADLINE:
                        printf("sharp2 deadline occurred\n");
                        break;
        }
}

void recvDICallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        robomon* ptr = (robomon*)recvCallBackParam;
        QEvent* event = new QEvent(EVENT_DI);
  switch (info->status) {
    case NEW_DATA:
        QCoreApplication::postEvent(ptr, event);        
        break;
    case DEADLINE:
      printf("DI deadline occurred\n");
      break;
  }
}

void recvIRCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        robomon* ptr = (robomon*)recvCallBackParam;
        QEvent* event = new QEvent(EVENT_IR);
  switch (info->status) {
    case NEW_DATA:
        QCoreApplication::postEvent(ptr, event);        
        break;
    case DEADLINE:
      printf("IR deadline occurred\n");
      break;
  }
}

void recvMsCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
//      robomon* ptr = (robomon*)recvCallBackParam;
//      QEvent* event = new QEvent(EVENT_IR);
        switch (info->status) {
                case NEW_DATA:
//                      printf("motion status left=%x, right=%x\n",ms.err_left,ms.err_right);
//                      QCoreApplication::postEvent(ptr, event);        
                        break;
                case DEADLINE:
                        printf("motion status deadline occurred\n");
                        break;
        }
}

void recvAccelCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        unsigned int *instance=(unsigned int*)vinstance;
//      robomon* ptr = (robomon*)recvCallBackParam;
//      QEvent* event = new QEvent(QEvent::User);
  switch (info->status) {
    case NEW_DATA:
        printf("accel: %d, %d, %d\n",instance[0]>>16,instance[1]>>16,instance[2]>>16);
        //QCoreApplication::postEvent(ptr, event);      
        //printf("data: %d\n",instance[0][0]);  
        break;
    case DEADLINE:
      printf("accel deadline occurred\n");
      break;
  }
}

void recvMotionStatusCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
//      robomon* ptr = (robomon*)recvCallBackParam;
//      QEvent* event = new QEvent(QEvent::User);
        switch (info->status) {
                case NEW_DATA:
                        printf("motion status: %x, %x\n",ms.err_left,ms.err_right);
        //QCoreApplication::postEvent(ptr, event);      
        //printf("data: %d\n",instance[0][0]);  
                        break;
                case DEADLINE:
                        printf("motion status deadline occurred\n");
                        break;
        }
}

void *orte_function( void *ptr )
{
        ORTEInit();
        ORTEVerbositySetOptions("ALL.0");
        d=ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN,NULL,NULL,ORTE_FALSE);
        
        if (!d) {
                printf("ORTEDomainAppCreate failed!\n");
        }
        publisherCreateServo(ptr);      
//      subscriberAccelCreate(NULL);
        publisherDOCreate(NULL);
        publisherMotorCreate(NULL);
        subscriberCreateDI(ptr);
        subscriberCreateIR(ptr);
        subscriberSharp1Create(ptr);
        subscriberSharp2Create(ptr);
        subscriberPositionCreate(ptr);
        subscriberCreateDI(ptr);
//      subscriberCreateMotionStatus(NULL);
        return NULL;    
}

---