void CGroundStationSim::WriteToRadio()
{
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMSTAT_ACK);
vecMsg.push_back(PROT_SIMSTAT_BROADCAST_MSG); // The radio msg to be broadcasted has to go last (after the ACK)!
if (!SendBuffer.empty())
{
ToCont(SendBuffer.front(), vecMsg);
SendBuffer.pop_front();
}
std::cout << "to sim: ";
dobots::print(vecMsg.begin(), vecMsg.end());
writeToSim(vecMsg);
}
void CGroundStationSim::Tick()
{
// int* cmd = readCommand(false);
// if (cmd != NULL)
// {
//
// }
VecMsg = readSim(false);
if (!VecMsg->empty())
{
std::cout << "GroundStation " << ModuleId << " from SIM: ";
dobots::print(VecMsg->begin(), VecMsg->end());
VecMsgType::iterator it = VecMsg->begin();
while (it != VecMsg->end())
{
int type = *it++;
//std::cout << "Type=" << type << std::endl;
switch (type)
{
case PROT_SIMCMD_RADIO_ROUND_START:
{
// Our turn to send messages
RadioRoundState = RADIO_STATE_ROUND_START;
WriteToRadio();
break;
}
case PROT_SIMCMD_RADIO_ROUND_BROADCAST_MSG:
{
// Received messages from other uavs
RadioRoundState = RADIO_STATE_ROUND_SENDRECEIVE;
RadioMsg bmsg;
it = FromCont(bmsg, it, VecMsg->end());
std::cout << "GroundStation " << ModuleId << " received bmsg: " << bmsg << std::endl;
ReceiveBuffer.push_back(bmsg);
break;
}
case PROT_SIMCMD_RADIO_ROUND_END:
{
//RadioRoundState = RADIO_STATE_ROUND_END; // At this state we wait for other parts to update before going to IDLE
ReadReceiveBuffer();
RadioRoundState = RADIO_STATE_ROUND_IDLE; // At this state the write buffer can be filled with new msgs.
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMSTAT_ACK);
std::cout << "to sim: ";
dobots::print(vecMsg.begin(), vecMsg.end());
writeToSim(vecMsg);
// Fill outbuffer with "new" msg
WriteToOutBuffer(CmdMsg);
break;
}
}
}
VecMsg->clear();
// Send ack when we received the msgs from other uavs
if (RadioRoundState == RADIO_STATE_ROUND_SENDRECEIVE)
{
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMSTAT_ACK);
std::cout << "to sim: ";
dobots::print(vecMsg.begin(), vecMsg.end());
writeToSim(vecMsg);
}
}
VecMsg = readFromGuiInterface(false);
if (!VecMsg->empty())
{
std::cout << "RADIO " << ModuleId << " from GuiInterface: ";
dobots::print(VecMsg->begin(), VecMsg->end());
// Should have more protocol here?
VecMsgType::iterator it = VecMsg->begin();
while (it != VecMsg->end())
{
int type = *it++;
//std::cout << "Type=" << type << std::endl;
switch (type)
{
case RADIO_MSG_RELAY_CMD:
{
RadioMsgRelayCmd cmdMsg;
it = FromCont(cmdMsg, it, VecMsg->end());
CmdMsg.Data.Data[1].Cmd = cmdMsg;
break;
}
}
}
VecMsg->clear();
}
usleep(config.TickTime);
}
