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); }
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); }