00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <deque>
00010
00011 #include <messages.h>
00012 #include <iostream>
00013 #include "DEBUG.h"
00014 using namespace FastEcslent;
00015 using namespace std;
00016 void FastEcslent::printMessageHeader(FastEcslent::Header h) {
00017 DEBUG(std::cout << "Type: " << (int) h.msgType
00018 << " Time: " << (int) h.millisecondsFromStart
00019 << " MessageStructSize: " << (int) h.sizeOfStruct
00020 << " NumberOfStructs : " << (int) h.numberOfStructs
00021 << std::endl;)
00022 flush(cout);
00023 }
00024
00025 void FastEcslent::printState(State *s){
00026 DEBUG(cout << "State of: " << s->id << endl
00027 << "Pos: (" << s->px << ", " << s->py << ", " << s->pz << ") " << endl
00028 << "Vel: (" << s->vx << ", " << s->vy << ", " << s->vz << ") " << endl
00029 << "Dh: " << s->dh << ", Ds: " << s->ds << endl
00030 << "Yaw:" << s->yaw << endl;)
00031 }
00032
00033 void FastEcslent::printMessageData(Message *m){
00034
00035 int offset = 0;
00036 int *ptr;
00037 State *sptr;
00038
00039 for (int i = 0; i < m->head.numberOfStructs; i++){
00040 switch ((int) m->head.msgType){
00041 case 0:
00042 DEBUG(cout << "Message type 0" << endl;)
00043 offset += InfoSize;
00044 break;
00045 case 1:
00046 DEBUG(cout << "Message type 1" << endl;)
00047 sptr = (State *) m->data;
00048 printState(sptr + i);
00049
00050 break;
00051 case 2:
00052 DEBUG(cout << "Before data" << endl; flush(cout);)
00053 ptr = (int *) m->data;
00054 DEBUG(cout << "IRM: Id: " << *(ptr + i) << endl;)
00055 flush(cout);
00056 offset += sizeof(int);
00057
00058 break;
00059 default:
00060 DEBUG(cout << "Unknown message type" << endl;)
00061 break;
00062 }
00063 }
00064
00065 }
00066 void FastEcslent::printMessage(Message *m){
00067 printMessageHeader(m->head);
00068 printMessageData(m);
00069 flush(cout);
00070
00071 }