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