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