void CSim::CmdStart()
{
VecMsgType vecMsg;
UavIterType it;
int id=0;
for (it=Uavs.begin(); it != Uavs.end(); ++it, ++id)
{
//it->UavData.UavData.UavId = id; // Should be set at init
vecMsg.clear();
it->UavData.UavData.Geom.Init();
it->UavData.UavData.Geom.Pos.x() = config.GroundStationX;
it->UavData.UavData.Geom.Pos.y() = config.GroundStationY;
vecMsg.push_back(PROT_SIMCMD_SET_GEOM);
//geom.Pos << (id+0)*50, (id+0)*50, (id+0)*10;
ToCont(it->UavData.UavData.Geom, vecMsg);
//vecMsg.push_back(PROT_SIMCMD_SET_BATTERY);
//vecMsg.push_back(40*60 + rand()%5); // 40 - 44 minutes battery time
//vecMsg.push_back((i+35)*60);
it->UavData.UavData.State = UAVSTATE_LANDED;
vecMsg.push_back(PROT_SIMCMD_SET_STATE);
vecMsg.push_back(it->UavData.UavData.State);
if (it->AutoPilotSim)
{
writeAutoPilotCommand(id, vecMsg);
it->WaitForReplyAutoPilot = true;
}
else
it->WaitForReplyAutoPilot = false;
// Each 2 minutes 2 uavs liftoff (with 20s in between)
//it->TakeOffTime = int(id/2)*120.0 + id*20 + rand()%10;
it->TakeOffTime = id*5.0;
if (id == UAVS_NUM)
it->TakeOffTime = 0;
//it->TakeOffTime = 0;
it->RadioWorks = false;
it->WaitForReplyRadio = false;
// Add a dummy wp
WayPoint wp;
wp.to << config.GroundStationX, config.GroundStationY, 0;
it->UavData.WayPoints.push_back(wp);
BroadcastMsgs[id].clear();
}
RadioRoundState = RADIO_STATE_ROUND_IDLE;
LastRadioRoundStartTime = 0;
Running = true;
StartTimeStamp = get_cur_1ms();
LastTimeStepTimeStamp = get_cur_1ms();
}
void CSim::CmdLand()
{
UavIterType itUav;
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
int id = itUav->UavData.UavData.UavId;
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMCMD_LAND);
writeRadioCommand(id, vecMsg);
}
}
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 CWpPlanner::Plan()
{
std::vector<FitnessGaussian2D> coverage;
float heightAdjustment(0);
std::vector<int> bestChoices;
std::vector<WayPoint> bestChoicesWp;
float heading;
Position pos;
WayPointsStruct curWps;
int id;
UAVState state;
UAVState oldState;
float batteryTimeLeft;
LandingStruct landing;
float minHeight;
float maxHeight;
bool enablePlanner;
{
// Copy current states
boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf);
heading = MapSelf->UavData.Geom.Heading.angle();
pos = MapSelf->UavData.Geom.Pos;
curWps = MapSelf->WayPoints;
id = MapSelf->UavData.UavId;
state = MapSelf->UavData.State;
oldState = MapSelf->PreviousState;
batteryTimeLeft = MapSelf->UavData.BatteryTimeLeft;
landing = MapSelf->GsCmd.Landing;
minHeight = MapSelf->GsCmd.HeightMin;
maxHeight = MapSelf->GsCmd.HeightMax;
enablePlanner = MapSelf->GsCmd.EnablePlanner;
}
if (!enablePlanner)
{
if (config.Debug > 0)
std::cout << get_cur_1ms() << " wp planner is disabled" << std::endl;
return;
}
UAVState newState = state;
UAVState newOldState = oldState;
/*
{
// Copy relevant gaussians to vector, so we don't lock for too long
// Take an area with radius: max_path_radius + 3*sigma coverage
// This is oversized, take the heading into account
float radius = config.NumWayPointPerPlan * config.WayPointDT * CRUISE_SPEED + 3*FIT_COV_SIGMA_X;
boost::interprocess::scoped_lock<MapMutexType> lockCoverage(*MutexCoverage);
FitnessCoverage->Get(coverage, pos.x()-radius, pos.x()+radius, pos.y()-radius, pos.y()+radius);
}
*/
if (config.Debug > 1)
{
std::cout << "Curwp num=" << curWps.WayPointsNum << " pos=[" << pos.transpose() << "]"
<< " state=" << state << " oldState=" << oldState;
}
// When flying normally: check current plan for collisions etc. And check if the plan is still long enough.
// If no collisions etc. and plan is long enough, we don't need a new plan.
bool plan = true;
if (newState != UAVSTATE_WAITING_TO_LAND &&
newState != UAVSTATE_LANDING &&
newState != UAVSTATE_LANDED &&
newState != UAVSTATE_TAKING_OFF)
{
if (CheckCurPlan(heightAdjustment, newState, newOldState, pos, heading, id, curWps))
{
plan = false;
//return;
}
if (newState == UAVSTATE_COLLISION_AVOIDING && state != UAVSTATE_COLLISION_AVOIDING)
newOldState = state;
}
if (config.Debug > 1)
std::cout << std::endl;
// Update state in map self
if (state != newState || oldState != newOldState)
{
boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf);
MapSelf->UavData.State = newState;
MapSelf->PreviousState = newOldState;
state = newState;
oldState = newOldState;
}
// When flying normally: check batteries
if (newState != UAVSTATE_GOING_HOME &&
newState != UAVSTATE_WAITING_TO_LAND &&
newState != UAVSTATE_LANDING &&
newState != UAVSTATE_LANDED &&
newState != UAVSTATE_TAKING_OFF)
{
float distLeft = pos.x() - landing.Pos.x();
distLeft += pos.y() - landing.Pos.y();
float timeNeeded = config.BatteryCriticalTime + distLeft/config.CruiseSpeed;
if (batteryTimeLeft < timeNeeded)
{
if (newState == UAVSTATE_COLLISION_AVOIDING)
newOldState = UAVSTATE_GOING_HOME;
else
newState = UAVSTATE_GOING_HOME;
}
}
// Fly height: default height + adjustment for collision avoidance
pos.z() = minHeight + id * (maxHeight-minHeight) / (UAVS_NUM-1);
pos.z() += heightAdjustment;
switch (newState)
{
case UAVSTATE_GOING_HOME:
{
// Fly directly to land circle
WayPoint wp;
GetLandWp(wp, landing, pos.z());
bestChoicesWp.push_back(wp);
// Check if we are at the land circle yet
Position dPos(pos);
dPos.x() -= wp.to.x();
dPos.y() -= wp.to.y();
dPos.z() = 0;
if (dPos.norm() < landing.Radius + 50) // TODO: magic number
newState = UAVSTATE_WAITING_TO_LAND;
break;
}
case UAVSTATE_WAITING_TO_LAND:
{
// Check if we can land: no other uavs below us landing or waiting to land
bool land=true;
{
boost::interprocess::scoped_lock<MapMutexType> lockUavs(*MutexUavs);
MapUavIterType it;
for (it = MapUavs->begin(); it != MapUavs->end(); ++it)
{
if ((it->second.data.UavId < id && it->second.data.State == UAVSTATE_WAITING_TO_LAND)
|| it->second.data.State == UAVSTATE_LANDING)
{
if (config.Debug > 0)
std::cout << "Waiting for uav " << it->second.data.UavId << " to land" << std::endl;
land = false;
break;
}
}
}
if (land)
{
if (config.Debug > 0)
std::cout << "My turn to land" << std::endl;
newState = UAVSTATE_LANDING;
VecMsgType vecMsg;
vecMsg.push_back(PROT_AP_SET_MODE);
vecMsg.push_back(AP_PROT_MODE_LAND);
writeToAutoPilot(vecMsg);
}
break;
}
case UAVSTATE_LANDING:
{
// Landing is performed by auto pilot
// Check if no other UAVs below us are landing, if so: abort landing!
bool land=true;
{
boost::interprocess::scoped_lock<MapMutexType> lockUavs(*MutexUavs);
MapUavIterType it;
for (it = MapUavs->begin(); it != MapUavs->end(); ++it)
{
if (it->second.data.State == UAVSTATE_LANDING)
{
if (config.Debug > 0)
std::cout << "Waiting for uav " << it->second.data.UavId << " to land" << std::endl;
land = false;
break;
}
}
}
if (!land)
{
if (config.Debug > 0)
std::cout << "Aborting landing" << std::endl;
newState = UAVSTATE_WAITING_TO_LAND;
VecMsgType vecMsg;
vecMsg.push_back(PROT_AP_SET_MODE);
vecMsg.push_back(AP_PROT_MODE_WP);
writeToAutoPilot(vecMsg);
// TODO: Assume the waypoint is still in autopilot memory or set it again?
WayPoint wp;
GetLandWp(wp, landing, pos.z());
bestChoicesWp.push_back(wp);
}
break;
}
case UAVSTATE_LANDED:
{
// Can't do much now :)
break;
}
default:
{
if (plan)
// Make a new plan
PlanStep(bestChoicesWp, bestChoices, config.NumWayPointPerPlan, config.WayPointDT, pos, heading, coverage);
break;
}
}
// Update state in map self
if (state != newState || oldState != newOldState)
{
boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf);
MapSelf->UavData.State = newState;
MapSelf->PreviousState = newOldState;
if (newState == UAVSTATE_LANDING)
MapSelf->RequestedAPModeByWP = AP_PROT_MODE_LAND;
else
MapSelf->RequestedAPModeByWP = AP_PROT_MODE_WP;
}
// When avoiding collision, set max vertical speed
if ((newState == UAVSTATE_COLLISION_AVOIDING) && (!bestChoices.empty()))
{
bestChoicesWp.rbegin()->VerticalSpeed = (heightAdjustment>0) ? config.MaxVertSpeed : -config.MaxVertSpeed;
}
// Send new waypoints to the autopilot
if (!bestChoicesWp.empty())
{
WayPointsStruct wps;
std::vector<WayPoint>::reverse_iterator itChoicesWp;
for (itChoicesWp = bestChoicesWp.rbegin(); itChoicesWp != bestChoicesWp.rend(); ++itChoicesWp)
wps.push_back(*itChoicesWp);
if (config.Debug > 0)
{
std::cout << "bestchoices=";
dobots::print(bestChoices.rbegin(), bestChoices.rend());
dobots::print(bestChoicesWp.rbegin(), bestChoicesWp.rend());
}
VecMsgType vecMsg;
vecMsg.push_back(PROT_AP_SET_WAYPOINTS);
ToCont(wps, vecMsg);
writeToAutoPilot(vecMsg);
}
}
void CSim::Tick()
{
IntMsg = readCommand(false);
if (IntMsg != NULL)
{
switch(*IntMsg)
{
case CMD_INIT:
CmdInit();
break;
case CMD_START:
CmdStart();
break;
case CMD_STOP:
CmdLand();
break;
}
}
// ------------------------------------------------------------------------------
// Read radio
// ------------------------------------------------------------------------------
UavIterType itUav;
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
int id = itUav->UavData.UavData.UavId;
VecMsg = readRadioState(id, false);
if (!VecMsg->empty())
{
std::cout << "SIM from Radio " << id << ": ";
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_SIMSTAT_ACK:
{
itUav->WaitForReplyRadio = false;
break;
}
case PROT_SIMSTAT_BROADCAST_MSG:
{
// This only works when we assume that the broadcast msg is at the end of vecmsg
//UavIterType itUav2;
if (it != VecMsg->end() && itUav->RadioWorks)
{
// Only add the msg to connected neighbours
//for (itUav2 = Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
for (int i = 0; i < itUav->UavData.NeighBours.ConnectedNeighboursNum; ++i)
{
//int id2 = itUav2->UavData.UavData.UavId;
int id2 = itUav->UavData.NeighBours.ConnectedNeighbours[i];
BroadcastMsgs[id2].push_back(PROT_SIMCMD_RADIO_ROUND_BROADCAST_MSG);
BroadcastMsgs[id2].insert(BroadcastMsgs[id2].end(), it, VecMsg->end());
}
// Debug test
RadioMsg bmsg;
it = FromCont(bmsg, it, VecMsg->end());
if (it != VecMsg->end())
std::cout << "ERROR! " << bmsg << std::endl;
std::cout << "Adding bmsg: " << bmsg << std::endl;
}
break;
}
}
}
VecMsg->clear();
}
}
// ------------------------------------------------------------------------------
// Read new auto pilot state (uav position etc)
// ------------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (!itUav->AutoPilotSim)
continue;
int id = itUav->UavData.UavData.UavId;
VecMsg = readAutoPilotState(id, false);
if (!VecMsg->empty())
{
std::cout << "SIM from AP " << id << ": ";
dobots::print(VecMsg->begin(), VecMsg->end());
// if (Uavs[i].UavData.UavData.State == UAVSTATE_LANDED)
// return; // Nothing to read here :)
VecMsgType::iterator it = VecMsg->begin();
while (it != VecMsg->end())
{
int type = *it++;
//std::cout << "Type=" << type << std::endl;
switch (type)
{
case PROT_SIMSTAT_ACK:
{
itUav->WaitForReplyAutoPilot = false;
break;
}
case PROT_SIMSTAT_GEOM:
{
//UavGeomStruct geom;
//FromCont(geom, it, VecMsg->end());
it = FromCont(itUav->UavData.UavData.Geom, it, VecMsg->end());
break;
}
// case PROT_SIMSTAT_GEOM_EXTRA:
// {
// itUav->UavData.Heading = *it++;
// itUav->UavData.RollAngle = *it++;
// break;
// }
case PROT_SIMSTAT_BATTERY:
{
itUav->UavData.UavData.BatteryTimeLeft = *it++;
break;
}
case PROT_SIMSTAT_WP:
{
it = FromCont(itUav->UavData.WayPoints[0], it, VecMsg->end());
break;
}
}
}
VecMsg->clear();
}
}
if (Running)
{
// --------------------------------------------------------------------------
// Check if can take the next step in the radio round
// --------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->WaitForReplyRadio)
{
// std::cout << "waiting for radio reply " << itUav->UavData.UavData.UavId << std::endl;
return;
}
}
// One radio round
// state = idle, waiting for time
// ==> start --- state = idle -> start
// <== bmsg, ack --- state = start -> send/receive
// ==> bmsgs --- state = send/receive
// <== ack --- state = send/receive -> end
// ==> end --- state = end
// <== ack --- state = end -> idle, wait for time
if (RadioRoundState != RADIO_STATE_ROUND_IDLE)
std::cout << "RadioRoundState=" << RadioRoundState << std::endl;
switch (RadioRoundState)
{
case RADIO_STATE_ROUND_START:
{
// Radios received the start command and replied with their msg
// Send msgs to all uavs with working radio
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
int id = itUav->UavData.UavData.UavId;
if (itUav->RadioWorks && !BroadcastMsgs[id].empty())
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(id, BroadcastMsgs[id]);
}
BroadcastMsgs[id].clear();
}
RadioRoundState = RADIO_STATE_ROUND_SENDRECEIVE;
return; // return, not break, since we have to wait for replies
}
case RADIO_STATE_ROUND_SENDRECEIVE:
{
// Tell all uavs that the radio round is over
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMCMD_RADIO_ROUND_END);
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(itUav->UavData.UavData.UavId, vecMsg);
}
RadioRoundState = RADIO_STATE_ROUND_END;
return; // return, not break, since we have to wait for replies
}
case RADIO_STATE_ROUND_END:
{
// All uavs received the end of the round, radio goes idle
RadioRoundState = RADIO_STATE_ROUND_IDLE;
break;
}
}
VecMsgType vecMsg;
// --------------------------------------------------------------------------
// After some time waiting it's time for a new radio round
// --------------------------------------------------------------------------
if (LastRadioRoundStartTime+config.RadioRoundTime < CurTime)
{
LastRadioRoundStartTime = CurTime;
RadioRoundState = RADIO_STATE_ROUND_START;
vecMsg.push_back(PROT_SIMCMD_RADIO_ROUND_START);
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(itUav->UavData.UavData.UavId, vecMsg);
}
return; // return, since we have to wait for replies
}
// --------------------------------------------------------------------------
// Check if we can take the next time step: shouldn't be waiting for any ACK (radio or autopilot)
// --------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->WaitForReplyAutoPilot || itUav->WaitForReplyRadio)
{
// std::cout << "waiting for reply " << itUav->UavData.UavData.UavId << std::endl;
return;
}
}
#ifdef SIM_REALTIME
// Simulation should go real time
if (get_duration(LastTimeStepTimeStamp, get_cur_1ms()) < 1000*config.TimeStep)
return;
LastTimeStepTimeStamp = get_cur_1ms();
#endif
// If we arrived here, all uavs have taken their time step and reported back the new state
// Update the neighbours of each UAV
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->UavData.NeighBours.clear();
// if (!itUav->AutoPilotSim)
// {
// // UAV has unknown position, let's just say it's connected to everything
// UavIterType itUav2;
// for (itUav2=Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
// if (itUav != itUav2)
// itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
// continue;
// }
Position p1(itUav->UavData.UavData.Geom.Pos);
if (itUav->UavData.UavData.UavId == UAVS_NUM)
p1 << config.GroundStationX, config.GroundStationY, 0;
Position p2, diff;
UavIterType itUav2;
for (itUav2=Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
{
// Uav can't listen to it's own msgs
if (itUav == itUav2)
continue;
// Position unknown of one of the UAVs: assume connected
if ((!itUav->AutoPilotSim && itUav->UavData.UavData.UavId != UAVS_NUM)
|| (!itUav2->AutoPilotSim && itUav2->UavData.UavData.UavId != UAVS_NUM))
{
itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
}
// Check distance
else
{
if (itUav2->UavData.UavData.UavId == UAVS_NUM)
p2 << config.GroundStationX, config.GroundStationY, 0;
else
p2 = itUav2->UavData.UavData.Geom.Pos;
diff = p1 - p2;
if (diff.dot(diff) <= config.RadioRange*config.RadioRange)
{
itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
}
}
}
//std::cout << "uav " << it->UavData.UavData.UavId << " neighbours: ";
//dobots::print(it->UavData.NeighBours.ConnectedNeighbours, it->UavData.NeighBours.ConnectedNeighbours + it->UavData.NeighBours.ConnectedNeighboursNum);
}
// Write current states of uavs to output file
FileOut << get_cur_1ms() << " ";
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->UavData.UavData.UavId == UAVS_NUM)
{
itUav->UavData.UavData.Geom.Pos << config.GroundStationX, config.GroundStationY, 0;
itUav->UavData.WayPoints[0].to << config.GroundStationX, config.GroundStationY, 0;
}
else if (!itUav->AutoPilotSim)
continue;
FileOut << itUav->UavData.UavData.UavId << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.x() << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.y() << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.z() << " ";
FileOut << itUav->UavData.UavData.Geom.Heading.angle() << " ";
FileOut << itUav->UavData.WayPoints[0].to.x() << " ";
FileOut << itUav->UavData.WayPoints[0].to.y() << " ";
FileOut << itUav->UavData.WayPoints[0].to.z() << " ";
//file << autoPilotSim->RollAngle.angle();
int neighbours = 0;
for (int i=0; i<itUav->UavData.NeighBours.ConnectedNeighboursNum; ++i)
neighbours |= 1 << itUav->UavData.NeighBours.ConnectedNeighbours[i];
FileOut << neighbours << " ";
}
FileOut << std::endl;
// Check for end of simulation time
if (CurTime > SimTime)
{
std::cout << std::endl << "----- End of simulation -----" << std::endl;
std::cout << SimTime << "s simulated in " << get_duration(StartTimeStamp, get_cur_1ms())/1000 << "s" << std::endl;
exit(1);
}
#ifndef SIM_REALTIME
// Give UAVs some time to calculate
// Wp planner takes about 25 ms
// Should have planning at at least 4 Hz, so 1 plan per 25 timesteps
// So each timestep should at least take 1ms
usleep(1*1000);
//usleep(10*1000); // 10ms = 0.01s = 0.1*timestep
//usleep(50*1000); // 50ms = 0.05s = 0.5*timestep
#endif
// Now that we've written all the latest states, we can go to the next time step
std::cout << std::endl << "----- Next time step " << CurTime << " -----" << std::endl;
// Command autopilot to take a time step
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
vecMsg.clear();
int id = itUav->UavData.UavData.UavId;
if (itUav->UavData.UavData.State == UAVSTATE_LANDED)
{
std::cout << id << " takeofftime=" << itUav->TakeOffTime << std::endl;
if (CurTime > itUav->TakeOffTime)
{
itUav->UavData.UavData.State = UAVSTATE_FLYING;
itUav->RadioWorks = true;
vecMsg.push_back(PROT_SIMCMD_SET_STATE);
vecMsg.push_back(UAVSTATE_FLYING);
}
}
if (!itUav->AutoPilotSim)
continue;
vecMsg.push_back(PROT_SIMCMD_TIMESTEP);
vecMsg.push_back(config.TimeStep);
itUav->WaitForReplyAutoPilot = true;
writeAutoPilotCommand(id, vecMsg);
}
// for (int i=0; i<Uavs.size(); ++i)
// {
// writeAutoPilotCommand(i, vecMsg);
// }
// Increase time
CurTime += config.TimeStep;
}
usleep(config.TickTime);
}
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);
}
void CGroundStationSim::ReadReceiveBuffer()
{
UavStruct uav;
while (!ReceiveBuffer.empty())
{
for (int i=0; i<RADIO_NUM_RELAY_PER_MSG; ++i)
{
switch (ReceiveBuffer.front().Data.Data[i].MessageType)
{
case RADIO_MSG_RELAY_POS:
{
uav.UavId = ReceiveBuffer.front().Data.Data[i].Pos.UavId -1;
// Ignore invalid uav IDs and own messages
if ((uav.UavId > -1) && (uav.UavId != UavId))
{
uav.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Pos);
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i] << " === " << uav << std::endl;
VecMsgType vecMsg;
vecMsg.push_back(PROT_RADIO_MSG_RELAY);
ToCont(ReceiveBuffer.front().Data.Data[i], vecMsg);
writeToMapUavs(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_POS);
ToCont(ReceiveBuffer.front().Data.Data[i].Pos, vecMsg);
writeToGuiInterface(vecMsg);
}
break;
}
case RADIO_MSG_RELAY_FIRE:
{
FireStruct fire;
VecMsgType vecMsg;
fire.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Fires.Fire[0]);
if (fire.UavId > -1)
{
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i].Fires.Fire[0] << std::endl;
vecMsg.push_back(PROT_FIRE_STRUCT);
ToCont(fire, vecMsg);
writeToMapFire(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_FIRE);
ToCont(ReceiveBuffer.front().Data.Data[i].Fires.Fire[0], vecMsg);
writeToGuiInterface(vecMsg);
}
fire.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Fires.Fire[1]);
if (fire.UavId > -1)
{
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i].Fires.Fire[1] << std::endl;
vecMsg.clear();
vecMsg.push_back(PROT_FIRE_STRUCT);
ToCont(fire, vecMsg);
writeToMapFire(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_FIRE);
ToCont(ReceiveBuffer.front().Data.Data[i].Fires.Fire[1], vecMsg);
writeToGuiInterface(vecMsg);
}
break;
}
case RADIO_MSG_RELAY_CMD:
{
// Ignore
break;
}
}
}
ReceiveBuffer.pop_front();
}
}
