src/robomon/src2/robomon_orte.cpp

   1 /*
   2  * robomon_orte.cpp                     07/10/31
   3  *
   4  * Collection of functions to manage ORTE stuffs.
   5  *
   6  * Copyright: (c) 2007 DCE Eurobot Dragon Team
   7  *            CTU FEE - Department of Control Engineering
   8  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
   9  * License: GNU GPL v.2
  10  */
  11
  12 #include <orte.h>
  13 #ifdef CONFIG_ORTE_RTL
  14   #include <linux/module.h>
  15   #include <posix/pthread.h>
  16   #define printf rtl_printf
  17 #elif defined CONFIG_ORTE_RTAI
  18   #include <linux/module.h>
  19   #include <rtai/compat.h>
  20   #define printf rt_printk
  21 #else
  22   #include <stdio.h>
  23 #endif
  24
  25 #include <RobotType.h>
  26 #include <RobotTypeArr.h>
  27 #include <QCoreApplication>
  28
  29 #include "robomon_orte.h"
  30
  31 int robomon_orte_init(void *arg, struct orte_data *data)
  32 {
  33         ORTEInit();
  34         ORTEVerbositySetOptions("ALL.0");
  35         data->orteDomain = ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN,
  36                                         NULL, NULL, ORTE_FALSE);
  37
  38         if (!data->orteDomain) {
  39                 printf("ORTEDomainAppCreate failed!\n");
  40                 return -1;
  41         }
  42
  43         return 0;
  44 }
  45
  46 /* ----------------------------------------------------------------------
  47  * PUBLISHERS
  48  * ---------------------------------------------------------------------- */
  49 void sendMotorCallBack(const ORTESendInfo *info,
  50                         void *vinstance, void *arg)
  51 {
  52         struct motionSpeed_type *instance = (struct motionSpeed_type *)vinstance;
  53
  54         switch (info->status) {
  55                 case NEED_DATA:
  56                         break;
  57                 case CQL:  /* criticalQueueLevel */
  58                         break;
  59         }
  60 }
  61
  62 void createMotorPublisher(void *arg, struct orte_data *data)
  63 {
  64         NtpTime persistence, delay;
  65
  66         motionSpeed_type_register(data->orteDomain);
  67         NTPTIME_BUILD(persistence, 3);
  68         NTPTIME_BUILD(delay, 1);
  69
  70         data->publisherMotor = ORTEPublicationCreate(
  71                         data->orteDomain, "motor", "motionSpeed",
  72                         &data->orteMotionSpeed, &persistence, 1,
  73                         sendMotorCallBack,  NULL, &delay);
  74 }
  75
  76 void sendServoCallBack(const ORTESendInfo *info,
  77                         void *vinstance, void *arg)
  78 {
  79         struct stateServa_type *instance = (struct stateServa_type *)vinstance;
  80
  81         switch (info->status) {
  82                 case NEED_DATA:
  83                         break;
  84                 case CQL:  /* criticalQueueLevel */
  85                         break;
  86         }
  87 }
  88
  89 void createServoPublisher(void *arg, struct orte_data *data)
  90 {
  91         NtpTime persistence, delay;
  92
  93         stateServa_type_register(data->orteDomain);
  94         NTPTIME_BUILD(persistence, 3);
  95         NTPTIME_BUILD(delay, 1);
  96
  97         data->publisherServos = ORTEPublicationCreate(
  98                         data->orteDomain, "serva", "stateServa",
  99                         &data->orteServosStatus, &persistence, 1,
 100                         sendServoCallBack, arg, &delay);
 101 }
 102
 103 void sendSharpLongsCallBack(const ORTESendInfo *info,
 104                         void *vinstance, void *arg)
 105 {
 106         struct sharpLongs_type *instance = (struct sharpLongs_type*)vinstance;
 107
 108         switch (info->status) {
 109                 case NEED_DATA:
 110                         break;
 111                 case CQL:  /* criticalQueueLevel */
 112                         break;
 113         }
 114 }
 115
 116 void createSharpLongsPublisher(void *arg, struct orte_data *data)
 117 {
 118         NtpTime persistence, delay;
 119
 120         sharpLongs_type_register(data->orteDomain);
 121         NTPTIME_BUILD(persistence, 3);
 122         NTPTIME_BUILD(delay, 1);
 123
 124         data->publisherSharpLong = ORTEPublicationCreate(
 125                         data->orteDomain, "sharpSouper", "sharpLongs",
 126                         &data->orteSharpOpponent, &persistence, 2,
 127                         sendSharpLongsCallBack, NULL, &delay);
 128 }
 129
 130 void sendSharpShortsCallBack(const ORTESendInfo *info,
 131                         void *vinstance, void *arg)
 132 {
 133         struct sharpShorts_type *instance = (struct sharpShorts_type*)vinstance;
 134
 135         switch (info->status) {
 136                 case NEED_DATA:
 137                         break;
 138                 case CQL:  /* criticalQueueLevel */
 139                         break;
 140         }
 141 }
 142
 143 void createSharpShortsPublisher(void *arg, struct orte_data *data)
 144 {
 145         NtpTime persistence, delay;
 146
 147         sharpShorts_type_register(data->orteDomain);
 148         NTPTIME_BUILD(persistence, 3);
 149         NTPTIME_BUILD(delay, 1);
 150
 151         data->publisherSharpShort = ORTEPublicationCreate(
 152                         data->orteDomain, "sharpLahve", "sharpShorts",
 153                         &data->orteSharpWaste, &persistence, 2,
 154                         sendSharpShortsCallBack, NULL, &delay);
 155 }
 156
 157 void createDOPublisher(void *arg, struct orte_data *data)
 158 {
 159         NtpTime persistence, delay;
 160
 161         ORTETypeRegisterAdd(data->orteDomain, "DOMsg", NULL, NULL,
 162                                 NULL, sizeof(data->stateDO));
 163         NTPTIME_BUILD(persistence, 3);
 164         NTPTIME_BUILD(delay, 1);
 165
 166         data->publisherDO = ORTEPublicationCreate(
 167                         data->orteDomain, "DO", "DOMsg", &data->stateDO,
 168                         &persistence, 1, NULL,  NULL, &delay);
 169 }
 170
 171 void sendPowerControlCallBack(const ORTESendInfo *info,
 172                         void *vinstance, void *arg)
 173 {
 174         struct pwr_ctrl_type *instance =
 175                         (struct pwr_ctrl_type *)vinstance;
 176
 177         switch (info->status) {
 178                 case NEED_DATA:
 179                         break;
 180                 case CQL:  /* criticalQueueLevel */
 181                         break;
 182         }
 183 }
 184
 185 void createPowerControlPublisher(void *arg, struct orte_data *data)
 186 {
 187         NtpTime persistence, delay;
 188
 189         pwr_ctrl_type_register(data->orteDomain);
 190         NTPTIME_BUILD(persistence, 3);
 191         NTPTIME_BUILD(delay, 1);
 192
 193         data->publisherPowerControl = ORTEPublicationCreate(
 194                         data->orteDomain, "pwr_ctrl", "pwr_ctrl",
 195                         &data->ortePowerControl, &persistence, 1,
 196                         sendPowerControlCallBack, NULL, &delay);
 197 }
 198
 199 /* ----------------------------------------------------------------------
 200  * SUBSCRIBERS
 201  * ---------------------------------------------------------------------- */
 202 void receiveMotionStatusCallBack(const ORTERecvInfo *info,
 203                         void *vinstance, void *arg)
 204 {
 205         switch (info->status) {
 206                 case NEW_DATA:
 207                         POST_QEVENT(arg, QEV_MOTION_STATUS);
 208                         break;
 209                 case DEADLINE:
 210                         printf("motion status deadline occurred\n");
 211                         break;
 212         }
 213 }
 214
 215 void createMotionStatusSubscriber(void *arg, struct orte_data *data)
 216 {
 217         ORTESubscription *s;
 218         NtpTime deadline,minimumSeparation;
 219
 220         motionStatus_type_register(data->orteDomain);
 221         NTPTIME_BUILD(deadline, 10);
 222         NTPTIME_BUILD(minimumSeparation, 0);
 223
 224         s = ORTESubscriptionCreate(
 225                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 226                         "motion_status", "motionStatus",
 227                         &data->orteMotionStatus, &deadline,
 228                         &minimumSeparation, receiveMotionStatusCallBack,
 229                         arg, IPADDRESS_INVALID);
 230 }
 231
 232 void receiveActualPositionCallBack(const ORTERecvInfo *info,
 233                         void *vinstance, void *arg)
 234 {
 235         switch (info->status) {
 236                 case NEW_DATA:
 237                         POST_QEVENT(arg, QEV_ACTUAL_POSITION);
 238                         break;
 239                 case DEADLINE:
 240                         printf("pos deadline occurred\n");
 241                         break;
 242         }
 243 }
 244
 245 void createActualPositionSubscriber(void *arg, struct orte_data *data)
 246 {
 247         ORTESubscription *s;
 248         NtpTime deadline, minimumSeparation;
 249
 250         position_type_register(data->orteDomain);
 251         NTPTIME_BUILD(deadline,10);
 252         NTPTIME_BUILD(minimumSeparation,0);
 253
 254         s = ORTESubscriptionCreate(
 255                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 256                         "pos", "position", &data->orteActPos, &deadline,
 257                         &minimumSeparation, receiveActualPositionCallBack,
 258                         arg, IPADDRESS_INVALID);
 259 }
 260
 261 void receiveEstimatedPositionCallBack(const ORTERecvInfo *info,
 262                         void *vinstance, void *arg)
 263 {
 264         switch (info->status) {
 265                 case NEW_DATA:
 266                         POST_QEVENT(arg, QEV_ESTIMATED_POSITION);
 267                         break;
 268                 case DEADLINE:
 269                         printf("est_pos deadline occurred\n");
 270                         break;
 271         }
 272 }
 273
 274 void createEstimatedPositionSubscriber(void *arg, struct orte_data *data)
 275 {
 276         ORTESubscription *s;
 277         NtpTime deadline,minimumSeparation;
 278
 279         position_type_register(data->orteDomain);
 280         NTPTIME_BUILD(deadline, 10);
 281         NTPTIME_BUILD(minimumSeparation, 0);
 282
 283         s = ORTESubscriptionCreate(
 284                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 285                         "est_pos", "position",  &data->orteEstPos,
 286                         &deadline, &minimumSeparation,
 287                         receiveEstimatedPositionCallBack, arg,
 288                         IPADDRESS_INVALID);
 289 }
 290
 291 void receiveSharpLongsCallBack(const ORTERecvInfo *info,
 292                         void *vinstance, void *arg)
 293 {
 294         switch (info->status) {
 295                 case NEW_DATA:
 296                         POST_QEVENT(arg, QEV_SHARP_LONGS);
 297                         break;
 298                 case DEADLINE:
 299                         printf("sharp1 deadline occurred\n");
 300                         break;
 301         }
 302 }
 303 void createSharpLongsSubscriber(void *arg, struct orte_data *data)
 304 {
 305         ORTESubscription *s;
 306         NtpTime deadline,minimumSeparation;
 307
 308         sharpLongs_type_register(data->orteDomain);
 309         NTPTIME_BUILD(deadline, 10);
 310         NTPTIME_BUILD(minimumSeparation, 0);
 311         s = ORTESubscriptionCreate(
 312                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 313                         "sharpSouper", "sharpLongs", &data->orteSharpLongs, //FIXME
 314                         &deadline, &minimumSeparation,
 315                         receiveSharpLongsCallBack, arg, IPADDRESS_INVALID);
 316 }
 317
 318 void receiveSharpShortsCallBack(const ORTERecvInfo *info,
 319                         void *vinstance, void *arg)
 320 {
 321         switch (info->status) {
 322                 case NEW_DATA:
 323                         POST_QEVENT(arg, QEV_SHARP_SHORTS);
 324                         break;
 325                 case DEADLINE:
 326                         printf("sharp2 deadline occurred\n");
 327                         break;
 328         }
 329 }
 330
 331 void createSharpShortsSubscriber(void *arg, struct orte_data *data)
 332 {
 333         ORTESubscription *s;
 334         NtpTime deadline, minimumSeparation;
 335
 336         sharpShorts_type_register(data->orteDomain);
 337         NTPTIME_BUILD(deadline, 10);
 338         NTPTIME_BUILD(minimumSeparation, 0);
 339
 340         s = ORTESubscriptionCreate(
 341                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 342                         "sharpLahve", "sharpShorts", &data->orteSharpShorts,
 343                         &deadline, &minimumSeparation,
 344                         receiveSharpShortsCallBack, arg, IPADDRESS_INVALID);
 345 }
 346
 347 void receiveIRCallBack(const ORTERecvInfo *info,
 348                         void *vinstance, void *arg)
 349 {
 350         switch (info->status) {
 351                 case NEW_DATA:
 352                         POST_QEVENT(arg, QEV_IR);
 353                         break;
 354                 case DEADLINE:
 355                         printf("IR deadline occurred\n");
 356                         break;
 357         }
 358 }
 359
 360 void createIRSubscriber(void *arg, struct orte_data *data)
 361 {
 362         ORTESubscription *s;
 363         NtpTime deadline, minimumSeparation;
 364
 365         stateInnerIR_type_register(data->orteDomain);
 366         NTPTIME_BUILD(deadline, 10);
 367         NTPTIME_BUILD(minimumSeparation, 0);
 368
 369         s = ORTESubscriptionCreate(
 370                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 371                         "IR", "stateInnerIR", &data->orteStateInnerIR,
 372                         &deadline, &minimumSeparation, receiveIRCallBack,
 373                         arg, IPADDRESS_INVALID);
 374 }
 375
 376 void receiveDICallBack(const ORTERecvInfo *info,
 377                         void *vinstance, void *arg)
 378 {
 379         switch (info->status) {
 380                 case NEW_DATA:
 381                         POST_QEVENT(arg, QEV_DI);
 382                         break;
 383                 case DEADLINE:
 384                         printf("DI deadline occurred\n");
 385                         break;
 386         }
 387 }
 388
 389 void createDISubscriber(void *arg, struct orte_data *data)
 390 {
 391         ORTESubscription *s;
 392         NtpTime deadline, minimumSeparation;
 393
 394         stateDigIn_type_register(data->orteDomain);
 395         NTPTIME_BUILD(deadline, 10);
 396         NTPTIME_BUILD(minimumSeparation, 0);
 397
 398         s = ORTESubscriptionCreate(
 399                         data->orteDomain, IMMEDIATE, BEST_EFFORTS, "DI",
 400                         "stateDigIn", &data->orteStateDI, &deadline,
 401                         &minimumSeparation, receiveDICallBack,
 402                         arg, IPADDRESS_INVALID);
 403 }
 404
 405 void receiveLaserCallBack(const ORTERecvInfo *info,
 406                         void *vinstance, void *arg)
 407 {
 408         switch (info->status) {
 409                 case NEW_DATA:
 410                         POST_QEVENT(arg, QEV_LASER);
 411                         break;
 412                 case DEADLINE:
 413                         printf("laser deadline occurred\n");
 414                         break;
 415         }
 416 }
 417
 418 void createLaserSubscriber(void *arg, struct orte_data *data)
 419 {
 420         ORTESubscription *s;
 421         NtpTime deadline, minimumSeparation;
 422
 423         laserData_type_register(data->orteDomain);
 424         NTPTIME_BUILD(deadline, 10);
 425         NTPTIME_BUILD(minimumSeparation, 0);
 426
 427         s = ORTESubscriptionCreate(
 428                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 429                         "laser", "laserData", &data->orteLaserData,
 430                         &deadline, &minimumSeparation, receiveLaserCallBack,
 431                         arg, IPADDRESS_INVALID);
 432 }
 433
 434 void receiveAccelerometerCallBack(const ORTERecvInfo *info,
 435                         void *vinstance, void *arg)
 436 {
 437         unsigned int *instance = (unsigned int *)vinstance;
 438
 439         switch (info->status) {
 440                 case NEW_DATA:
 441                         printf("accel: %d, %d, %d\n",
 442                                         instance[0]>>16,
 443                                         instance[1]>>16,instance[2]>>16);
 444                         POST_QEVENT(arg, QEV_ACCELEROMETER);
 445                         break;
 446                 case DEADLINE:
 447                         printf("accel deadline occurred\n");
 448                         break;
 449         }
 450 }
 451
 452 void createAccelerometerSubscriber(void *arg, struct orte_data *data)
 453 {
 454         ORTESubscription *s;
 455         NtpTime deadline,minimumSeparation;
 456
 457         ORTETypeRegisterAdd(data->orteDomain, "AccelMsg", NULL, NULL,
 458                                 NULL, sizeof(data->accelRecv));
 459         NTPTIME_BUILD(deadline, 10);
 460         NTPTIME_BUILD(minimumSeparation, 0);
 461
 462         s = ORTESubscriptionCreate(
 463                         data->orteDomain, IMMEDIATE, BEST_EFFORTS, "accel",
 464                         "AccelMsg", &data->accelRecv, &deadline,
 465                         &minimumSeparation, receiveAccelerometerCallBack,
 466                         arg, IPADDRESS_INVALID);
 467 }
 468
 469 void receiveAccumulatorCallBack(const ORTERecvInfo *info,
 470                         void *vinstance, void *arg)
 471 {
 472         switch (info->status) {
 473                 case NEW_DATA:
 474                         POST_QEVENT(arg, QEV_ACCUMULATOR);
 475                         break;
 476                 case DEADLINE:
 477                         printf("accu deadline occurred\n");
 478                         break;
 479         }
 480 }
 481
 482 void createAccumulatorSubscriber(void *arg, struct orte_data *data)
 483 {
 484         ORTESubscription *s;
 485         NtpTime deadline, minimumSeparation;
 486
 487         accumulator_type_register(data->orteDomain);
 488         NTPTIME_BUILD(deadline, 10);
 489         NTPTIME_BUILD(minimumSeparation, 0);
 490
 491         s = ORTESubscriptionCreate(
 492                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 493                         "accu", "accumulator", &data->orteAccumulator,
 494                         &deadline, &minimumSeparation,
 495                         receiveAccumulatorCallBack, arg, IPADDRESS_INVALID);
 496 }
 497
 498 void receivePowerVoltageCallBack(const ORTERecvInfo *info,
 499                         void *vinstance, void *arg)
 500 {
 501         switch (info->status) {
 502                 case NEW_DATA:
 503                         //printf("voltage: %2.2f\n",);
 504                         POST_QEVENT(arg, QEV_POWER_VOLTAGE);
 505                         break;
 506                 case DEADLINE:
 507                         printf("pwr_act_volts deadline occurred\n");
 508                         break;
 509         }
 510 }
 511
 512 void createPowerVoltageSubscriber(void *arg, struct orte_data *data)
 513 {
 514         ORTESubscription *s;
 515         NtpTime deadline, minimumSeparation;
 516
 517         pwr_act_volts_type_register(data->orteDomain);
 518         NTPTIME_BUILD(deadline, 10);
 519         NTPTIME_BUILD(minimumSeparation, 0);
 520
 521         s = ORTESubscriptionCreate(
 522                         data->orteDomain, IMMEDIATE, BEST_EFFORTS,
 523                         "pwr_voltage", "pwr_act_volts",
 524                         &data->ortePowerVoltage, &deadline, &minimumSeparation,
 525                         receivePowerVoltageCallBack, arg, IPADDRESS_INVALID);
 526 }