Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/HostEthController.cpp

    r3 r15  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
     13//  purpose:    Base class for host side remote controls that talks to target
     14//  side through ethernet connection
    1415//
    1516//
    … …  
    3738using std::string;
    3839
    39 namespace flair
    40 {
    41 namespace sensor
    42 {
    43 
    44 HostEthController::HostEthController(const FrameworkManager* parent,string name,string _address,int _port,uint32_t period,uint32_t _bitsPerAxis,uint8_t priority) :
    45     Thread(parent,name,priority),IODevice(parent,name),tab(new Tab(parent->GetTabWidget(),name)),axisNumber(0),buttonNumber(0),targetAddress(_address),targetPort(_port),
    46     bitsPerAxis(_bitsPerAxis),dataFrameBuffer(NULL),meaningfulDataAvailable(false) {
    47     tabWidget=new TabWidget(tab->NewRow(),name);
    48 
    49     const bool blocking=true;
    50     controlSocket=new TcpSocket((Thread*)this,"eth controller control socket", blocking, !blocking);
    51     dataSocket=new Socket((Thread*)this,"eth controller data socket",_address+":"+std::to_string(_port+1));
    52     dataSender=new DataSender((Thread*)this, this, "eth controller data sender thread");
    53     dataSender->SetPeriodMS(period);
    54     dataSender->Start();
    55 
    56     //test binary
    57     /*
    58     uint16_t testValue=0b0111011010; //0x1DA
    59     char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; // 0xAB65B6
    60     writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
    61     Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
    62     */
    63     connectionEstablishedMutex=new Mutex((Thread*)this);
     40namespace flair {
     41namespace sensor {
     42
     43HostEthController::HostEthController(const FrameworkManager *parent,
     44                                     string name, string _address, int _port,
     45                                     uint32_t period, uint32_t _bitsPerAxis,
     46                                     uint8_t priority)
     47    : Thread(parent, name, priority), IODevice(parent, name),
     48      tab(new Tab(parent->GetTabWidget(), name)), axisNumber(0),
     49      buttonNumber(0), targetAddress(_address), targetPort(_port),
     50      bitsPerAxis(_bitsPerAxis), dataFrameBuffer(NULL),
     51      meaningfulDataAvailable(false) {
     52  tabWidget = new TabWidget(tab->NewRow(), name);
     53
     54  const bool blocking = true;
     55  controlSocket = new TcpSocket((Thread *)this, "eth controller control socket",
     56                                blocking, !blocking);
     57  dataSocket = new Socket((Thread *)this, "eth controller data socket",
     58                          _address + ":" + std::to_string(_port + 1));
     59  dataSender =
     60      new DataSender((Thread *)this, this, "eth controller data sender thread");
     61  dataSender->SetPeriodMS(period);
     62  dataSender->Start();
     63
     64  // test binary
     65  /*
     66  uint16_t testValue=0b0111011010; //0x1DA
     67  char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; //
     68  0xAB65B6
     69  writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
     70  Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
     71  */
     72  connectionEstablishedMutex = new Mutex((Thread *)this);
    6473}
    6574
    6675HostEthController::~HostEthController() {
     76  SafeStop();
     77  Join();
     78
     79  if (!getFrameworkManager()->ConnectionLost())
     80    delete tab;
     81}
     82
     83void HostEthController::DrawUserInterface() {
     84  Tab *plotTab = new Tab(tabWidget, "Measures");
     85  axisPlot = new DataPlot1D *[axisNumber];
     86  for (unsigned int i = 0; i < axisNumber; i++) {
     87    // Start a new row or add up to the current row? We try to keep a 4/3 ratio
     88    unsigned int columns = sqrt(4.0 * axisNumber / 3.0);
     89    LayoutPosition *position;
     90    if (i % columns == 0) {
     91      position = plotTab->NewRow();
     92    } else {
     93      position = plotTab->LastRowLastCol();
     94    }
     95    axisPlot[i] = new DataPlot1D(position, axis->Name(i, 0),
     96                                 -(1 << (nativeBitsPerAxis - 1)) * 1.2,
     97                                 (1 << (nativeBitsPerAxis - 1)) * 1.5);
     98    axisPlot[i]->AddCurve(axis->Element(i));
     99  }
     100  // we don't plot the button state for now
     101}
     102
     103string HostEthController::GetAxisDescription(unsigned int axis) {
     104  return string("axis") + std::to_string(axis);
     105}
     106
     107string HostEthController::GetButtonDescription(unsigned int button) {
     108  return string("button") + std::to_string(button);
     109}
     110
     111bool HostEthController::ControllerInitialization() {
     112
     113  buttonOffset = (axisNumber * bitsPerAxis) / 8;
     114  if ((axisNumber * bitsPerAxis) % 8 != 0)
     115    buttonOffset++;
     116  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
     117  if ((buttonNumber % 8) != 0)
     118    dataFrameSize++;
     119  dataFrameBuffer = new char[dataFrameSize];
     120  return true;
     121}
     122
     123void HostEthController::SendControllerInfo() {
     124  // send axis info
     125  controlSocket->WriteUInt32((uint32_t)axisNumber, 0);
     126  controlSocket->WriteUInt32(bitsPerAxis, 0);
     127  for (unsigned int i = 0; i < axisNumber; i++) {
     128    // Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d
     129    // bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
     130    int stringLength = GetAxisDescription(i).length();
     131    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
     132    controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength, 0);
     133  }
     134
     135  // send button info
     136  controlSocket->WriteUInt32((uint32_t)buttonNumber, 0);
     137  for (unsigned int i = 0; i < buttonNumber; i++) {
     138    int stringLength = GetButtonDescription(i).length();
     139    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
     140    controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,
     141                               0);
     142  }
     143}
     144
     145bool HostEthController::ConnectedWithTarget() {
     146  char message[1024];
     147  ssize_t sent, received;
     148  static bool connectionEstablished = false;
     149
     150  connectionEstablishedMutex->GetMutex();
     151  if (!connectionEstablished &&
     152      controlSocket->Connect(targetPort, targetAddress, 10)) {
     153    Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
     154    SendControllerInfo();
     155    connectionEstablished = true;
     156  }
     157  connectionEstablishedMutex->ReleaseMutex();
     158  return connectionEstablished;
     159}
     160
     161void HostEthController::Run() {
     162  static int divider = 0;
     163  Message *msgControllerAction = new Message(1024);
     164  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
    67165    SafeStop();
    68     Join();
    69 
    70     if(!getFrameworkManager()->ConnectionLost()) delete tab;
    71 }
    72 
    73 void HostEthController::DrawUserInterface() {
    74     Tab* plotTab=new Tab(tabWidget,"Measures");
    75     axisPlot=new DataPlot1D *[axisNumber];
    76     for (unsigned int i=0; i<axisNumber;i++) {
    77         //Start a new row or add up to the current row? We try to keep a 4/3 ratio
    78         unsigned int columns=sqrt(4.0*axisNumber/3.0);
    79         LayoutPosition *position;
    80         if (i%columns==0) {
    81             position=plotTab->NewRow();
    82         } else {
    83             position=plotTab->LastRowLastCol();
     166    Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
     167  }
     168
     169  if (buttonNumber % 8 != 0) {
     170    SafeStop();
     171    Thread::Err("Button number is not multiple of 8\n");
     172  }
     173
     174  while (!ToBeStopped()) {
     175    // Thread::Info("Debug: entering acquisition loop\n");
     176    if (getFrameworkManager()->ConnectionLost() == true)
     177      SafeStop();
     178
     179    if (IsDataFrameReady()) { // wait for next data frame
     180      meaningfulDataAvailable = true;
     181      GetAxisData();
     182      GetButtonData();
     183
     184      if (ConnectedWithTarget()) {
     185        // read for commands from the target (remote control state change
     186        // requests such as rumble or led on/off)
     187        ssize_t bytesReceived =
     188            controlSocket->RecvMessage((char *)msgControllerAction->buffer,
     189                                       msgControllerAction->bufferSize);
     190        if (bytesReceived > 0) {
     191          // if the message is a cnx exit request we manage it here, if not it
     192          // will be managed by the derived class
     193          ControllerAction action;
     194          memcpy(&action, msgControllerAction->buffer,
     195                 sizeof(ControllerAction));
     196          if (action == ControllerAction::Exit) {
     197            // Thread::Info("Debug: exit request received from server\n");
     198            SafeStop();
     199          }
     200          ProcessMessage(msgControllerAction);
    84201        }
    85         axisPlot[i]=new DataPlot1D(position,axis->Name(i,0),-(1<<(nativeBitsPerAxis-1))*1.2,(1<<(nativeBitsPerAxis-1))*1.5);
    86         axisPlot[i]->AddCurve(axis->Element(i));
    87     }
    88     //we don't plot the button state for now
    89 }
    90 
    91 string HostEthController::GetAxisDescription(unsigned int axis) {
    92     return string("axis")+std::to_string(axis);
    93 }
    94 
    95 string HostEthController::GetButtonDescription(unsigned int button) {
    96     return string("button")+std::to_string(button);
    97 }
    98 
    99 bool HostEthController::ControllerInitialization() {
    100 
    101     buttonOffset=(axisNumber*bitsPerAxis)/8;
    102     if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
    103     dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
    104     if ((buttonNumber%8)!=0) dataFrameSize++;
    105     dataFrameBuffer=new char[dataFrameSize];
    106     return true;
    107 }
    108 
    109 void HostEthController::SendControllerInfo() {
    110     //send axis info
    111     controlSocket->WriteUInt32((uint32_t)axisNumber,0);
    112     controlSocket->WriteUInt32(bitsPerAxis,0);
    113     for (unsigned int i=0; i<axisNumber; i++) {
    114         //Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
    115         int stringLength=GetAxisDescription(i).length();
    116         controlSocket->WriteUInt32((uint32_t)stringLength,0);
    117         controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength,0);
    118     }
    119 
    120     //send button info
    121     controlSocket->WriteUInt32((uint32_t)buttonNumber,0);
    122     for (unsigned int i=0; i<buttonNumber; i++) {
    123         int stringLength=GetButtonDescription(i).length();
    124         controlSocket->WriteUInt32((uint32_t)stringLength,0);
    125         controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,0);
    126     }
    127 }
    128 
    129 bool HostEthController::ConnectedWithTarget() {
    130     char message[1024];
    131     ssize_t sent,received;
    132     static bool connectionEstablished=false;
    133 
    134     connectionEstablishedMutex->GetMutex();
    135     if (!connectionEstablished && controlSocket->Connect(targetPort,targetAddress,10)) {
    136         Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
    137         SendControllerInfo();
    138         connectionEstablished=true;
    139     }
    140     connectionEstablishedMutex->ReleaseMutex();
    141     return connectionEstablished;
    142 }
    143 
    144 void HostEthController::Run() {
    145     static int divider=0;
    146     Message *msgControllerAction=new Message(1024);
    147     if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
    148         SafeStop();
    149         Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
    150     }
    151 
    152     if(buttonNumber%8!=0) {
    153         SafeStop();
    154         Thread::Err("Button number is not multiple of 8\n");
    155     }
    156 
    157     while(!ToBeStopped()) {
    158         //Thread::Info("Debug: entering acquisition loop\n");
    159         if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
    160 
    161         if (IsDataFrameReady()) {  //wait for next data frame
    162             meaningfulDataAvailable=true;
    163             GetAxisData();
    164             GetButtonData();
    165 
    166             if (ConnectedWithTarget()) {
    167                 //read for commands from the target (remote control state change requests such as rumble or led on/off)
    168                 ssize_t bytesReceived=controlSocket->RecvMessage((char*)msgControllerAction->buffer,msgControllerAction->bufferSize);
    169                 if(bytesReceived>0) {
    170                     //if the message is a cnx exit request we manage it here, if not it will be managed by the derived class
    171                     ControllerAction action;
    172                     memcpy(&action,msgControllerAction->buffer,sizeof(ControllerAction));
    173                     if (action==ControllerAction::Exit) {
    174                         //Thread::Info("Debug: exit request received from server\n");
    175                         SafeStop();
    176                     }
    177                     ProcessMessage(msgControllerAction);
    178                 }
    179             }
    180         } else {//try to connect even if host is not sending anything
    181             ConnectedWithTarget();
    182         }
    183         //Thread::Info("Debug: exiting acquisition loop\n");
    184     }
    185 }
    186 
    187 bool HostEthController::writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits) {
    188     if (valueSizeInBits>16) return false;
    189     uint8_t remainingBitsToWrite=valueSizeInBits;
    190     //skip first bytes
    191     buffer+=offsetInBits/8;
    192     offsetInBits-=(offsetInBits/8)*8;
    193     while (remainingBitsToWrite>0) {
    194         uint8_t remainingBitsInByteBeforeWrite=8-offsetInBits;
    195         uint8_t bitsToWrite=remainingBitsToWrite<remainingBitsInByteBeforeWrite?remainingBitsToWrite:remainingBitsInByteBeforeWrite;
    196         uint8_t remainingBitsInByteAfterWrite=remainingBitsInByteBeforeWrite-bitsToWrite;
    197         //write in the current byte
    198         uint8_t byteMask=((1<<bitsToWrite)-1)<<remainingBitsInByteAfterWrite;
    199         (*buffer)&=~byteMask;
    200         uint16_t valueMask=(1<<remainingBitsToWrite)-1;
    201         (*buffer)|=((value&valueMask)>>(remainingBitsToWrite-bitsToWrite))<<remainingBitsInByteAfterWrite;
    202         //update state
    203         remainingBitsToWrite-=bitsToWrite;
    204         offsetInBits=(offsetInBits+bitsToWrite)%8;
    205         buffer++;
    206     }
    207     return true;
     202      }
     203    } else { // try to connect even if host is not sending anything
     204      ConnectedWithTarget();
     205    }
     206    // Thread::Info("Debug: exiting acquisition loop\n");
     207  }
     208}
     209
     210bool HostEthController::writeBits(uint16_t value, uint8_t valueSizeInBits,
     211                                  char *buffer, uint8_t offsetInBits) {
     212  if (valueSizeInBits > 16)
     213    return false;
     214  uint8_t remainingBitsToWrite = valueSizeInBits;
     215  // skip first bytes
     216  buffer += offsetInBits / 8;
     217  offsetInBits -= (offsetInBits / 8) * 8;
     218  while (remainingBitsToWrite > 0) {
     219    uint8_t remainingBitsInByteBeforeWrite = 8 - offsetInBits;
     220    uint8_t bitsToWrite = remainingBitsToWrite < remainingBitsInByteBeforeWrite
     221                              ? remainingBitsToWrite
     222                              : remainingBitsInByteBeforeWrite;
     223    uint8_t remainingBitsInByteAfterWrite =
     224        remainingBitsInByteBeforeWrite - bitsToWrite;
     225    // write in the current byte
     226    uint8_t byteMask = ((1 << bitsToWrite) - 1)
     227                       << remainingBitsInByteAfterWrite;
     228    (*buffer) &= ~byteMask;
     229    uint16_t valueMask = (1 << remainingBitsToWrite) - 1;
     230    (*buffer) |= ((value & valueMask) >> (remainingBitsToWrite - bitsToWrite))
     231                 << remainingBitsInByteAfterWrite;
     232    // update state
     233    remainingBitsToWrite -= bitsToWrite;
     234    offsetInBits = (offsetInBits + bitsToWrite) % 8;
     235    buffer++;
     236  }
     237  return true;
    208238}
    209239
    210240void HostEthController::BuildDataFrame() {
    211     //int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
    212     for (unsigned int i=0;i<axisNumber;i++) {
    213         //We shift value to always be positive (so that division/multiplication by power of 2 can easily be done with bit shifts)
    214         uint16_t shiftedNativeAxisValue=axis->Value(i,0)+(1<<(nativeBitsPerAxis-1));
    215         //int16_t nativeAxisValue=testValue[i];
    216         uint16_t scaledAxisValue;
    217         if (bitsPerAxis>nativeBitsPerAxis) {
    218             scaledAxisValue=shiftedNativeAxisValue<<(bitsPerAxis-nativeBitsPerAxis);
    219         } else {
    220             scaledAxisValue=shiftedNativeAxisValue>>(nativeBitsPerAxis-bitsPerAxis);
    221         }
    222         //Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
    223         unsigned int offsetInBits=i*bitsPerAxis;
    224         writeBits(scaledAxisValue,bitsPerAxis,dataFrameBuffer,offsetInBits);
    225     }
    226     //Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0], dataFrameBuffer[1], dataFrameBuffer[2]);
    227 
    228     int currentButton=0;
    229     uint8_t buttonArray[buttonNumber/8];
    230     for (unsigned int i=0;i<buttonNumber/8;i++) {
    231         buttonArray[i]=0;
    232         for(unsigned int j=0;j<8;j++) {
    233             bool buttonValue=button->Value(currentButton,0);
    234             if(buttonValue) buttonArray[i]+=1<<j;
    235             currentButton++;
    236         }
    237 
    238         dataSocket->HostToNetwork((char *)&buttonArray[i],sizeof(uint8_t));
    239         memcpy(dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),&buttonArray[i],sizeof(uint8_t));
    240     }
    241 }
    242 
    243 HostEthController::DataSender::DataSender(Object *parent,HostEthController* _hostEthController,string name,uint8_t priority):Thread(parent,name,priority),hostEthController(_hostEthController) {
    244 
    245 }
     241  // int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
     242  for (unsigned int i = 0; i < axisNumber; i++) {
     243    // We shift value to always be positive (so that division/multiplication by
     244    // power of 2 can easily be done with bit shifts)
     245    uint16_t shiftedNativeAxisValue =
     246        axis->Value(i, 0) + (1 << (nativeBitsPerAxis - 1));
     247    // int16_t nativeAxisValue=testValue[i];
     248    uint16_t scaledAxisValue;
     249    if (bitsPerAxis > nativeBitsPerAxis) {
     250      scaledAxisValue = shiftedNativeAxisValue
     251                        << (bitsPerAxis - nativeBitsPerAxis);
     252    } else {
     253      scaledAxisValue =
     254          shiftedNativeAxisValue >> (nativeBitsPerAxis - bitsPerAxis);
     255    }
     256    // Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis
     257    // value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
     258    unsigned int offsetInBits = i * bitsPerAxis;
     259    writeBits(scaledAxisValue, bitsPerAxis, dataFrameBuffer, offsetInBits);
     260  }
     261  // Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0],
     262  // dataFrameBuffer[1], dataFrameBuffer[2]);
     263
     264  int currentButton = 0;
     265  uint8_t buttonArray[buttonNumber / 8];
     266  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
     267    buttonArray[i] = 0;
     268    for (unsigned int j = 0; j < 8; j++) {
     269      bool buttonValue = button->Value(currentButton, 0);
     270      if (buttonValue)
     271        buttonArray[i] += 1 << j;
     272      currentButton++;
     273    }
     274
     275    dataSocket->HostToNetwork((char *)&buttonArray[i], sizeof(uint8_t));
     276    memcpy(dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
     277           &buttonArray[i], sizeof(uint8_t));
     278  }
     279}
     280
     281HostEthController::DataSender::DataSender(Object *parent,
     282                                          HostEthController *_hostEthController,
     283                                          string name, uint8_t priority)
     284    : Thread(parent, name, priority), hostEthController(_hostEthController) {}
    246285
    247286void HostEthController::DataSender::Run() {
    248     if(getFrameworkManager()->ErrorOccured()==true) {
    249         SafeStop();
    250     }
    251 
    252     while(!ToBeStopped()) {
    253         WaitPeriod();
    254         if (hostEthController->meaningfulDataAvailable && hostEthController->ConnectedWithTarget()) {
    255             //send the data
    256             hostEthController->BuildDataFrame();
    257             hostEthController->dataSocket->SendMessage(hostEthController->dataFrameBuffer,hostEthController->dataFrameSize);
    258         }
    259     }
     287  if (getFrameworkManager()->ErrorOccured() == true) {
     288    SafeStop();
     289  }
     290
     291  while (!ToBeStopped()) {
     292    WaitPeriod();
     293    if (hostEthController->meaningfulDataAvailable &&
     294        hostEthController->ConnectedWithTarget()) {
     295      // send the data
     296      hostEthController->BuildDataFrame();
     297      hostEthController->dataSocket->SendMessage(
     298          hostEthController->dataFrameBuffer, hostEthController->dataFrameSize);
     299    }
     300  }
    260301}
    261302
Note: See TracChangeset for help on using the changeset viewer.