예제 #1
0
void CBuzzController::ProcessOutMsgs() {
   /* Send robot id */
   CByteArray cData;
   cData << m_tBuzzVM->robot;
   /* Send messages from FIFO */
   do {
      /* Are there more messages? */
      if(buzzoutmsg_queue_isempty(m_tBuzzVM->outmsgs)) break;
      /* Get first message */
      buzzmsg_payload_t m = buzzoutmsg_queue_first(m_tBuzzVM->outmsgs);
      /* Make sure the next message fits the data buffer */
      if(cData.Size() + buzzmsg_payload_size(m) + sizeof(UInt16)
         >
         m_pcRABA->GetSize()) {
         buzzmsg_payload_destroy(&m);
         break;
      }
      /* Add message length to data buffer */
      cData << static_cast<UInt16>(buzzmsg_payload_size(m));
      /* Add payload to data buffer */
      cData.AddBuffer(reinterpret_cast<UInt8*>(m->data), buzzmsg_payload_size(m));
      /* Get rid of message */
      buzzoutmsg_queue_next(m_tBuzzVM->outmsgs);
      buzzmsg_payload_destroy(&m);
   } while(1);
   /* Pad the rest of the data with zeroes */
   while(cData.Size() < m_pcRABA->GetSize()) cData << static_cast<UInt8>(0);
   /* Send message */
   m_pcRABA->SetData(cData);
}
예제 #2
0
void CBuzzController::ProcessOutMsgs() {
   /* Process outgoing messages */
   buzzvm_process_outmsgs(m_tBuzzVM);
   /* Send robot id */
   CByteArray cData;
   cData << m_tBuzzVM->robot;
   /* Send messages from FIFO */
   do {
      /* Are there more messages? */
      if(buzzoutmsg_queue_isempty(m_tBuzzVM)) break;
      /* Get first message */
      buzzmsg_payload_t m = buzzoutmsg_queue_first(m_tBuzzVM);
      /* Make sure the message is smaller than the data buffer
       * Without this check, large messages would clog the queue forever
       */
      size_t unMsgSize = buzzmsg_payload_size(m) + sizeof(UInt16);
      if(unMsgSize < m_pcRABA->GetSize() - sizeof(UInt16)) {
         /* Make sure the next message fits the data buffer */
         if(cData.Size() + unMsgSize > m_pcRABA->GetSize()) {
            buzzmsg_payload_destroy(&m);
            break;
         }
         /* Add message length to data buffer */
         cData << static_cast<UInt16>(buzzmsg_payload_size(m));
         /* Add payload to data buffer */
         cData.AddBuffer(reinterpret_cast<UInt8*>(m->data), buzzmsg_payload_size(m));
      }
      else {
         RLOGERR << "Discarded oversize message ("
                 << unMsgSize
                 << " bytes). Max size is "
                 << m_pcRABA->GetSize() - sizeof(UInt16)
                 << " bytes."
                 << std::endl;
      }
      /* Get rid of message */
      buzzoutmsg_queue_next(m_tBuzzVM);
      buzzmsg_payload_destroy(&m);
   } while(1);
   /* Pad the rest of the data with zeroes */
   while(cData.Size() < m_pcRABA->GetSize()) cData << static_cast<UInt8>(0);
   /* Send message */
   m_pcRABA->SetData(cData);
}