Example #1
0
void
sendMessage(IMC::Message& msg)
{
  uint16_t rv = IMC::Packet::serialize(&msg, (uint8_t*)g_buffer, sizeof(g_buffer));
  g_sock.write(g_buffer, rv, g_addr, g_port);
  msg.toText(std::cerr);
}
Example #2
0
TEST(Reflection, Variable)
{
	AM_DECLARE_VAR(g_instance, int, "MyInteger", 2);
	ASSERT_TRUE(g_instance == 2);
	
	g_instance = 3;
	ASSERT_TRUE(g_instance == 3);

	AM_ASSIGN_VAR("MyInteger", 4);
	ASSERT_TRUE(g_instance == 4);
	
	AM_ASSIGN_VAR_STR("MyInteger", "5");
	ASSERT_TRUE(g_instance == 5);
	
	UDPWriteDesc writeDesc;
	writeDesc.port = 5005;
	writeDesc.address = "localhost";
	std::string str = "MyInteger 6";
	UDPSocket writeSocket;
	ASSERT_TRUE(writeSocket.connect(writeDesc));
	ASSERT_EQ(writeSocket.write(str.c_str(), str.length()), str.length());
	std::this_thread::sleep_for(std::chrono::seconds(1));
	ASSERT_TRUE(g_instance == 6);
	GlobalVariableHandler::destroyInstance();
	
}
Example #3
0
void sendEotPacket(UDPSocket & udpSocket, int seqNum, unsigned long remoteIp, unsigned int remotePort) {
	Packet *eotPacket = Packet::createEOT(seqNum);
	char *udpData = eotPacket->getUDPData();
	int writeBytes = udpSocket.write(remoteIp, remotePort, udpData, eotPacket->getUDPSize());
	ASSERT(writeBytes > 0);
	free(udpData);
	delete(eotPacket);
}
Example #4
0
static void
sendMsg(IMC::Message& msg, UDPSocket& sock, Address& dest, int port)
{
    DUNE::Utils::ByteBuffer bb;
    msg.setTimeStamp();
    IMC::Packet::serialize(&msg, bb);
    sock.write((const uint8_t*)bb.getBuffer(), msg.getSerializationSize(), dest, port);
    msg.toText(std::cout);
}
Example #5
0
void sendAckPacket(UDPSocket & udpSocket, int seqNum, unsigned long remoteIp, unsigned int remotePort) {
	Packet *ackPacket = Packet::createACK(seqNum);
	char *udpData = ackPacket->getUDPData();
	int writeBytes = udpSocket.write(remoteIp, remotePort, udpData, ackPacket->getUDPSize());
	LOG("ACK %i sent \n", seqNum);
	ASSERT(writeBytes > 0);
	free(udpData);
	delete(ackPacket);
}
Example #6
0
int
main(int argc, char** argv)
{
  if (argc < 4)
  {
    std::fprintf(stderr, "Usage: %s <destination host> <destination port> <entity> <true|false>\n", argv[0]);
    return 1;
  }

  Address dest(argv[1]);

  // Parse port.
  unsigned port = 0;
  if (!castLexical(argv[2], port))
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  if (port > 65535)
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  IMC::EntityParameter p;
  p.name = "Active";
  p.value = argv[4];

  IMC::SetEntityParameters msg;
  msg.name = argv[3];
  msg.params.push_back(p);
  msg.setTimeStamp();

  uint8_t bfr[1024] = {0};
  uint16_t rv = IMC::Packet::serialize(&msg, bfr, sizeof(bfr));

  UDPSocket sock;
  try
  {
    sock.write(bfr, rv, dest, port);

    fprintf(stderr, "Raw:");
    for (int i = 0; i < rv; ++i)
      fprintf(stderr, " %02X", bfr[i]);
    fprintf(stderr, "\n");

    msg.toText(std::cerr);
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return 1;
  }

  return 0;
}
Example #7
0
/** Submit an SMS for delivery to an IMSI. */
int sendsimple(int argc, char** argv, ostream& os)
{
	if (argc<4) return BAD_NUM_ARGS;

	char *IMSI = argv[1];
	char *srcAddr = argv[2];
	string rest = "";
	for (int i=3; i<argc; i++) rest = rest + argv[i] + " ";
	const char *txtBuf = rest.c_str();

	if (!isIMSI(IMSI)) {
		os << "Invalid IMSI. Enter 15 digits only.";
		return BAD_VALUE;
	}

	static UDPSocket sock(0,"127.0.0.1",gConfig.getNum("SIP.Local.Port"));

	static const char form[] =
		"MESSAGE sip:IMSI%[email protected] SIP/2.0\n"
		"Via: SIP/2.0/TCP 127.0.0.1;branch=%x\n"
		"Max-Forwards: 2\n"
		"From: %s <sip:%[email protected]:%d>;tag=%d\n"
		"To: sip:IMSI%[email protected]\n"
		"Call-ID: %[email protected]:%d\n"
		"CSeq: 1 MESSAGE\n"
		"Content-Type: text/plain\nContent-Length: %u\n"
		"\n%s\n";
	static char buffer[1500];
	snprintf(buffer,1499,form,
		IMSI, (unsigned)random(), srcAddr,srcAddr,sock.port(),(unsigned)random(), IMSI, (unsigned)random(),sock.port(), strlen(txtBuf), txtBuf);
	sock.write(buffer);

	os << "message submitted for delivery" << endl;

#if 0
	int numRead = sock.read(buffer,10000);
	if (numRead>=0) {
		buffer[numRead]='\0';
		os << "response: " << buffer << endl;
	} else {
		os << "timed out waiting for response";
	}
#endif

	return SUCCESS;
}
Example #8
0
void gWriteGSMTAP(unsigned ARFCN, unsigned TS, unsigned FN,
                  GSM::TypeAndOffset to, bool is_saach, bool ul_dln,
                  const char* data, unsigned int len,
		  unsigned wType, unsigned int defSCN)
{
	if (!(data && len && socketActive()))
		return;
	char buffer[MAX_UDP_LENGTH];
	int ofs = 0;
	if (!(ofs = buildHeader(buffer,MAX_UDP_LENGTH,ARFCN,TS,FN,
					to,is_saach,ul_dln,wType,defSCN)))
		return;

	// Add frame data
	::memcpy(&buffer[ofs],data,len);
	ofs += len;

	// Write the GSMTAP packet
	GSMTAPSocket.write(buffer, ofs);
}
Example #9
0
void gWriteGSMTAP(unsigned ARFCN, unsigned TS, unsigned FN,
                  GSM::TypeAndOffset to, bool is_saach,
				  bool ul_dln,	// (pat) This flag means uplink
                  const BitVector& frame,
				  unsigned wType)	// Defaults to GSMTAP_TYPE_UM
{
	// Check if GSMTap is enabled
	if (!socketActive()) return;

	char buffer[MAX_UDP_LENGTH];
	int ofs = 0;
	
	if (!(ofs = buildHeader(buffer,MAX_UDP_LENGTH,ARFCN,TS,FN,
					to,is_saach,ul_dln,wType,0)))
		return;
	// Add frame data
	frame.pack((unsigned char*)&buffer[ofs]);
	ofs += (frame.size() + 7) >> 3;

	// Write the GSMTAP packet
	GSMTAPSocket.write(buffer, ofs);
}
Example #10
0
TEST(Network, UDPSocket)
{
	UDPWriteDesc writeDesc;
	writeDesc.port = 5005;
	writeDesc.address = "localhost";
	
	UDPSocket writeSocket;
	ASSERT_TRUE(writeSocket.connect(writeDesc));
	std::string str = "Hello World";
	
	
	UDPReadDesc readDesc;
	readDesc.port = 5005;
	
	char buffer[1024];
	UDPSocket socket;
	ASSERT_TRUE(socket.connect(readDesc));
	ASSERT_EQ(writeSocket.write(str.c_str(), str.length()), str.length());
	ASSERT_EQ(socket.read(buffer, 1024), str.length());
	ASSERT_STREQ(str.c_str(), buffer);
	AM_LOG_SUCCESS("%s", buffer);
}
Example #11
0
int main() {
	//create tcp socket and establish a connection
    TCPServerSocket tcpServerSocket;

    int ret = tcpServerSocket.initialize();
    ASSERT(ret == 0);
    
    int serverUdpPort = tcpServerSocket.negotiate();
    ASSERT(serverUdpPort > 1024);

    tcpServerSocket.close();

    //open a udp socket and listen for str
    UDPSocket udpSocket;
    ret = udpSocket.open(serverUdpPort);
    ASSERT(ret == 0);
    LOG("Server opened UDP Port %i\n", serverUdpPort);

    char buf[1024];
    unsigned long remoteIp;
    int remotePort;
    
    //read, reverse string and send it back to client
	int readBytes = udpSocket.read(buf, 1024, remoteIp, remotePort);
	buf[readBytes] = '\0';
    ASSERT(readBytes > 0);
    LOG("Server Received: %s\n", buf);

    //Reverse the string and write to the buffer again
    str_reverse(buf);

    int writeBytes = udpSocket.write(remoteIp, remotePort, (const char *) buf, readBytes);
    ASSERT(writeBytes > 0);
    LOG("Server sent: %s\n", buf);

    udpSocket.close();

    return 0;
}
Example #12
0
int
main(int argc, char** argv)
{
  if (argc < 4)
  {
    fprintf(stderr, "DUNE message sender\n\n");

    if (argc == 2 && (!strcmp(argv[1], "-l") || !strcmp(argv[1], "--list")))
    {
      fprintf(stdout, "Available Messages:\n");
      fprintf(stdout, "  [A]: Abort, AcousticMessage, AcousticOperation, AcousticSystemsQuery\n");
      fprintf(stdout, "  [B]: Brake, ButtonEvent\n");
      fprintf(stdout, "  [C]: CacheControl, Calibration, ClockControl, ControlLoops\n");
      fprintf(stdout, "  [D]: DataSanity, DesiredControl, DesiredHeading, DesiredHeadingRate, DesiredPitch,\n");
      fprintf(stdout, "       DesiredSpeed, DesiredRoll, DesiredZ, DevCalibrationControl, DevDataText\n");
      fprintf(stdout, "  [E]: EmergencyControl, EntityList, EntityState, EntityActivationState, EstimatedState\n");
      fprintf(stdout, "  [F]: FuelLevel\n");
      fprintf(stdout, "  [G]: GpsFix, GpsFixRtk\n");
      fprintf(stdout, "  [H]: Heartbeat\n");
      fprintf(stdout, "  [I]: IridiumMsgTx\n");
      fprintf(stdout, "  [L]: LblConfig, LblRange, LeaderState, LeakSimulation, LogBookControl, LogBookEntry,\n");
      fprintf(stdout, "       LoggingControl\n");
      fprintf(stdout, "  [M]: MagneticField, MonitorEntityState\n");
      fprintf(stdout, "  [O]: OperationalLimits\n");
      fprintf(stdout, "  [P]: PlanControl, PlanGeneration, PopEntityParameters, PowerChannelControl,\n");
      fprintf(stdout, "       PowerChannelState, PushEntityParameters\n");
      fprintf(stdout, "  [Q]: QueryEntityInfo, QueryEntityParameters\n");
      fprintf(stdout, "  [R]: RegisterManeuver, RemoteActions, RemoteActionsRequest, ReplayControl, ReportControl,\n");
      fprintf(stdout, "       RestartSystem\n");
      fprintf(stdout, "  [S]: SaveEntityParameters, SetEntityParameters, SetLedBrightness, SetServoPosition,\n");
      fprintf(stdout, "       SetThrusterActuation, Sms, SoundSpeed\n");
      fprintf(stdout, "  [T]: Target, TeleoperationDone, Temperature, TextMessage, TrexCommand\n");
      fprintf(stdout, "  [U]: UASimulation\n");
      fprintf(stdout, "  [V]: VehicleCommand, VehicleMedium\n");
      return 1;
    }

    fprintf(stderr, "Usage:\n");
    fprintf(stderr, "  %s <destination host> <destination port> <abbrev> [arguments]\n\n", argv[0]);
    fprintf(stderr, "Options:\n");
    fprintf(stderr, "  -l, --list                            Print list of acceptable messages\n\n\n");
    return 1;
  }

  Address dest(argv[1]);

  // Parse port.
  unsigned port = 0;
  if (!castLexical(argv[2], port))
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  if (port > 65535)
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  IMC::Message* msg = NULL;

  if (strcmp(argv[3], "Abort") == 0)
  {
    msg = new IMC::Abort;

    if (argc == 5)
      msg->setDestination(atoi(argv[4]));
  }

  if (strcmp(argv[3], "AcousticMessage") == 0)
  {
    IMC::AcousticMessage* tmsg = new IMC::AcousticMessage;
    msg = tmsg;
    IMC::Message* imsg = IMC::Factory::produce(atoi(argv[4]));
    tmsg->message.set(*imsg);
    delete imsg;
  }

  if (strcmp(argv[3], "AcousticOperation") == 0)
  {
    IMC::AcousticOperation* tmsg = new IMC::AcousticOperation;
    msg = tmsg;
    tmsg->op = IMC::AcousticOperation::AOP_RANGE_RECVED;
    tmsg->system = argv[4];
    tmsg->range = atoi(argv[5]);
  }

  if (strcmp(argv[3], "AcousticSystemsQuery") == 0)
  {
    IMC::AcousticSystemsQuery* tmsg = new IMC::AcousticSystemsQuery;
    msg = tmsg;
  }

  if (strcmp(argv[3], "Brake") == 0)
  {
    IMC::Brake* tmsg = new IMC::Brake;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }

  if (strcmp(argv[3], "ButtonEvent") == 0)
  {
    IMC::ButtonEvent* tmsg = new IMC::ButtonEvent;
    msg = tmsg;
    tmsg->button = atoi(argv[4]);
    tmsg->value = atoi(argv[5]);
  }

  if (strcmp(argv[3], "CacheControl") == 0)
  {
    IMC::CacheControl* tmsg = new IMC::CacheControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }

  if (strcmp(argv[3], "Calibration") == 0)
  {
    IMC::Calibration* tmsg = new IMC::Calibration;
    tmsg->duration = (uint16_t)(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "ClockControl") == 0)
  {
    IMC::ClockControl* tmsg = new IMC::ClockControl;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->clock = atof(argv[5]);
    if (argc >= 7)
      tmsg->tz = atoi(argv[6]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "ControlLoops") == 0)
  {
    IMC::ControlLoops* tmsg = new IMC::ControlLoops;
    msg = tmsg;
    tmsg->enable = atoi(argv[4]) ? 1 : 0;
    tmsg->mask = atoi(argv[5]);
    tmsg->scope_ref = atoi(argv[6]);
  }

  if (strcmp(argv[3], "DataSanity") == 0)
  {
    IMC::DataSanity* tmsg = new IMC::DataSanity;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->sane = atoi(argv[5]);
  }

  if (strcmp(argv[3], "DesiredControl") == 0)
  {
    IMC::DesiredControl* tmsg = new IMC::DesiredControl;
    tmsg->k = atof(argv[4]);
    tmsg->m = atof(argv[5]);
    tmsg->n = atof(argv[6]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredHeading") == 0)
  {
    IMC::DesiredHeading* tmsg = new IMC::DesiredHeading;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredHeadingRate") == 0)
  {
    IMC::DesiredHeadingRate* tmsg = new IMC::DesiredHeadingRate;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredPitch") == 0)
  {
    IMC::DesiredPitch* tmsg = new IMC::DesiredPitch;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredSpeed") == 0)
  {
    IMC::DesiredSpeed* tmsg = new IMC::DesiredSpeed;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->speed_units = IMC::SUNITS_PERCENTAGE;
    else
      tmsg->speed_units = atoi(argv[5]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredRoll") == 0)
  {
    IMC::DesiredRoll* tmsg = new IMC::DesiredRoll;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredZ") == 0)
  {
    IMC::DesiredZ* tmsg = new IMC::DesiredZ;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->z_units = static_cast<IMC::ZUnits>(3);
    else
      tmsg->z_units = static_cast<IMC::ZUnits>(atoi(argv[5]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DevCalibrationControl") == 0)
  {
    IMC::DevCalibrationControl * tmsg = new IMC::DevCalibrationControl;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->op = atoi(argv[5]);
  }

  if (strcmp(argv[3], "DevDataText") == 0)
  {
    IMC::DevDataText * tmsg = new IMC::DevDataText;
    msg = tmsg;
    tmsg->value = argv[4];
  }

  if (strcmp(argv[3], "EmergencyControl") == 0)
  {
    IMC::EmergencyControl* tmsg = new IMC::EmergencyControl;
    tmsg->command = atoi(argv[4]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "EntityList") == 0)
  {
    IMC::EntityList* tmsg = new IMC::EntityList;
    msg = tmsg;
    tmsg->op = IMC::EntityList::OP_QUERY;
  }

  if (strcmp(argv[3], "EntityState") == 0)
  {
    IMC::EntityState* tmsg = new IMC::EntityState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = atoi(argv[5]);
  }

  if (strcmp(argv[3], "EntityActivationState") == 0)
  {
    IMC::EntityActivationState* tmsg = new IMC::EntityActivationState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = IMC::EntityActivationState::EAS_ACTIVE;
  }

  if (strcmp(argv[3], "EstimatedState") == 0)
  {
    IMC::EstimatedState* tmsg = new IMC::EstimatedState;
    msg = tmsg;
    tmsg->x = atof(argv[4]);
    tmsg->y = atof(argv[5]);
    tmsg->z = atof(argv[6]);
    tmsg->phi = 0.0;
    tmsg->theta = 0.0;
    tmsg->psi = 0.0;
    tmsg->u = 0.0;
    tmsg->v = 0.0;
    tmsg->w = 0.0;
    tmsg->p = 0.0;
    tmsg->q = 0.0;
    tmsg->r = 0.0;
    tmsg->lat = 0.0;
    tmsg->lon = 0.0;
    tmsg->depth = 0.0;
  }

  if (strcmp(argv[3], "FuelLevel") == 0)
  {
    IMC::FuelLevel* tmsg = new IMC::FuelLevel;
    msg = tmsg;
    tmsg->setSource(atof(argv[4]));
    tmsg->value = atof(argv[5]);
    tmsg->confidence = atof(argv[6]);
  }

  if (strcmp(argv[3], "GpsFix") == 0)
  {
    IMC::GpsFix* tmsg = new IMC::GpsFix;
    msg = tmsg;
    tmsg->type = IMC::GpsFix::GFT_DIFFERENTIAL;
    tmsg->satellites = 10;
    tmsg->validity = 0xFFFF;

    if (argc >= 5)
    {
      tmsg->lat = Angles::radians(atof(argv[4]));
      tmsg->lon = Angles::radians(atof(argv[5]));
    }
    else
    {
      // Leixões harbor location.
      tmsg->lat = 0.718803520085;
      tmsg->lon = -0.151951035032;
    }

    if (argc >= 7)
      tmsg->height = atof(argv[6]);
  }

  if (strcmp(argv[3], "GpsFixRtk") == 0)
  {
    IMC::GpsFixRtk* tmsg = new IMC::GpsFixRtk;
    msg = tmsg;
    tmsg->type = IMC::GpsFixRtk::RTK_FIXED;
    tmsg->satellites = 10;
    tmsg->iar_hyp = 1;
    tmsg->setSource(0x2c01);

    if (argc >= 5)
      tmsg->setSource(tmsg->getSource() + atoi(argv[4]));

    if (argc >= 6)
    {
      if (!strcmp(argv[5], "Float"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_FLOAT;
      }
      else if (!strcmp(argv[5], "Obs"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_OBS;
      }
      else if (!strcmp(argv[5], "None"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_NONE;
      }
      else
      {
        tmsg->type = IMC::GpsFixRtk::RTK_FIXED;
      }
    }
    if (argc >= 9)
    {
      tmsg->n = atof(argv[6]);
      tmsg->e = atof(argv[7]);
      tmsg->d = atof(argv[8]);

    }
    else
    {
      // Default location
      tmsg->n = 4.0;
      tmsg->e = 3.0;
      tmsg->d = -2.0;
    }

    if (argc == 7)
    {
      tmsg->iar_hyp = atoi(argv[6]);
    }
  }

  if (strcmp(argv[3], "Heartbeat") == 0)
  {
    IMC::Heartbeat* tmsg = new IMC::Heartbeat;

    if (argc > 4)
      tmsg->setSource(atoi(argv[4]));

    if (argc > 5)
      tmsg->setDestination(atoi(argv[5]));

    msg = tmsg;
  }

  if (strcmp(argv[3], "IridiumMsgTx") == 0)
  {
    IMC::IridiumMsgTx* tmsg = new IMC::IridiumMsgTx;
    msg = tmsg;
    tmsg->req_id = atoi(argv[4]);
    tmsg->ttl = atoi(argv[5]);
    std::string hex = String::fromHex(argv[6]);
    tmsg->data.assign(hex.begin(), hex.end());
  }

  if (strcmp(argv[3], "LblConfig") == 0)
  {
    IMC::LblConfig* tmsg = new IMC::LblConfig;
    msg = tmsg;
    tmsg->op = IMC::LblConfig::OP_SET_CFG;

    IMC::LblBeacon bc;
    bc.beacon = "benthos2";
    bc.lat = 0.71883274;
    bc.lon = -0.15194732;
    bc.depth = 3;
    tmsg->beacons.push_back(bc);
    bc.beacon = "benthos3";
    bc.lat = 0.71881068;
    bc.lon = -0.15192335;
    tmsg->beacons.push_back(bc);
  }

  if (strcmp(argv[3], "LblRange") == 0)
  {
    IMC::LblRange* tmsg = new IMC::LblRange;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->range = atoi(argv[5]);
  }

  if (strcmp(argv[3], "LeaderState") == 0)
  {
    IMC::LeaderState* tmsg = new IMC::LeaderState;
    msg = tmsg;
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->height = atof(argv[6]);
  }

  if (strcmp(argv[3], "LeakSimulation") == 0)
  {
    IMC::LeakSimulation* tmsg = new IMC::LeakSimulation;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
    msg = tmsg;
  }

  if (strcmp(argv[3], "LogBookControl") == 0)
  {
    IMC::LogBookControl* tmsg = new IMC::LogBookControl;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->htime = Time::Clock::getSinceEpoch() - atof(argv[5]);
    else
      tmsg->htime = -1;
    msg = tmsg;
  }

  if (strcmp(argv[3], "LogBookEntry") == 0)
  {
    IMC::LogBookEntry* tmsg = new IMC::LogBookEntry;
    msg = tmsg;
    tmsg->context = argv[4];
    tmsg->text = argv[5];
    tmsg->htime = Time::Clock::getSinceEpoch();
    if (argc > 6)
      tmsg->type = atoi(argv[6]);
    else
      tmsg->type = IMC::LogBookEntry::LBET_WARNING;
  }

  if (strcmp(argv[3], "LoggingControl") == 0)
  {
    IMC::LoggingControl* tmsg = new IMC::LoggingControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
    tmsg->name = argv[5];
  }

  if (strcmp(argv[3], "MagneticField") == 0)
  {
    IMC::MagneticField* tmsg = new IMC::MagneticField;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->x = atof(argv[5]);
    tmsg->y = atof(argv[6]);
    tmsg->z = atof(argv[7]);
  }

  if (strcmp(argv[3], "MonitorEntityState") == 0)
  {
    IMC::MonitorEntityState* tmsg = new IMC::MonitorEntityState;
    msg = tmsg;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
  }

  if (strcmp(argv[3], "OperationalLimits") == 0)
  {
    IMC::OperationalLimits* tmsg = new IMC::OperationalLimits;
    tmsg->mask = IMC::OPL_AREA;
    tmsg->lat = DUNE::Math::Angles::radians(atof(argv[4]));
    tmsg->lon = DUNE::Math::Angles::radians(atof(argv[5]));
    tmsg->orientation = DUNE::Math::Angles::radians(atof(argv[6]));
    tmsg->width = atof(argv[7]);
    tmsg->length = atof(argv[8]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "PlanControl") == 0)
  {
    IMC::PlanControl* tmsg = new IMC::PlanControl;
    tmsg->type = IMC::PlanControl::PC_REQUEST;
    tmsg->op = atoi(argv[4]);
    tmsg->plan_id = argv[5];
    if (argc >= 7)
      tmsg->flags = atoi(argv[6]);

    if (argc >= 8)
      tmsg->arg.set(IMC::Factory::produce(argv[7]));

    msg = tmsg;
  }

  if (strcmp(argv[3], "PlanGeneration") == 0)
  {
    IMC::PlanGeneration* tmsg = new IMC::PlanGeneration;
    msg = tmsg;
    tmsg->cmd = atoi(argv[4]);
    tmsg->op = atoi(argv[5]);
    tmsg->plan_id = argv[6];

    if (argc >= 8)
      tmsg->params = argv[7];
  }

  if (strcmp(argv[3], "PopEntityParameters") == 0)
  {
    IMC::PopEntityParameters* tmsg = new IMC::PopEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "PowerChannelControl") == 0)
  {
    IMC::PowerChannelControl* tmsg = new IMC::PowerChannelControl;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->op = atoi(argv[5]);
  }

  if (strcmp(argv[3], "PowerChannelState") == 0)
  {
    IMC::PowerChannelState* tmsg = new IMC::PowerChannelState;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->state = atoi(argv[5]);
  }

  if (strcmp(argv[3], "PushEntityParameters") == 0)
  {
    IMC::PushEntityParameters* tmsg = new IMC::PushEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "QueryEntityInfo") == 0)
  {
    IMC::QueryEntityInfo* tmsg = new IMC::QueryEntityInfo;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
  }

  if (strcmp(argv[3], "QueryEntityParameters") == 0)
  {
    IMC::QueryEntityParameters* tmsg = new IMC::QueryEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "RegisterManeuver") == 0)
  {
    IMC::RegisterManeuver* tmsg = new IMC::RegisterManeuver;
    msg = tmsg;
    tmsg->mid = atoi(argv[4]);
  }

  if (strcmp(argv[3], "RemoteActions") == 0)
  {
    IMC::RemoteActions* tmsg = new IMC::RemoteActions;
    msg = tmsg;
    tmsg->actions = argv[4];
  }

  if (strcmp(argv[3], "RemoteActionsRequest") == 0)
  {
    IMC::RemoteActionsRequest* tmsg = new IMC::RemoteActionsRequest;
    msg = tmsg;
    tmsg->op = IMC::RemoteActionsRequest::OP_QUERY;
  }

  if (strcmp(argv[3], "ReplayControl") == 0)
  {
    IMC::ReplayControl* tmsg = new IMC::ReplayControl;
    tmsg->op = atoi(argv[4]);
    if (tmsg->op == IMC::ReplayControl::ROP_START)
      tmsg->file = argv[5];
    msg = tmsg;
  }

  if (strcmp(argv[3], "ReportControl") == 0)
  {
    IMC::ReportControl* tmsg = new IMC::ReportControl;
    tmsg->op = atoi(argv[4]);
    tmsg->comm_interface = atoi(argv[5]);
    tmsg->period = atoi(argv[6]);
    tmsg->sys_dst = argv[7];
    msg = tmsg;
  }

  if (strcmp(argv[3], "RestartSystem") == 0)
  {
    IMC::RestartSystem* tmsg = new IMC::RestartSystem;
    msg = tmsg;
  }

  if (strcmp(argv[3], "SaveEntityParameters") == 0)
  {
    IMC::SaveEntityParameters* tmsg = new IMC::SaveEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "SetEntityParameters") == 0)
  {
    IMC::SetEntityParameters* tmsg = new IMC::SetEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];

    IMC::EntityParameter param;

    unsigned i = 4;

    while (1)
    {
      if (argc >= (int)i + 2)
      {
        ++i;
        param.name = argv[i];
        ++i;
        param.value = argv[i];

        tmsg->params.push_back(param);
      }
      else
      {
        break;
      }
    }
  }

  if (strcmp(argv[3], "SetLedBrightness") == 0)
  {
    IMC::SetLedBrightness* tmsg = new IMC::SetLedBrightness;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->value = atoi(argv[5]);
  }

  if (strcmp(argv[3], "SetServoPosition") == 0)
  {
    IMC::SetServoPosition* tmsg = new IMC::SetServoPosition;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }

  if (strcmp(argv[3], "SetThrusterActuation") == 0)
  {
    IMC::SetThrusterActuation* tmsg = new IMC::SetThrusterActuation;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }

  if (strcmp(argv[3], "Sms") == 0)
  {
    IMC::Sms* tmsg = new IMC::Sms;
    tmsg->number = argv[4];
    tmsg->timeout = atoi(argv[5]);
    tmsg->contents = argv[6];
    msg = tmsg;
  }

  if (strcmp(argv[3], "SoundSpeed") == 0)
  {
    IMC::SoundSpeed* tmsg = new IMC::SoundSpeed;
    msg = tmsg;
    tmsg->value = atoi(argv[4]);
  }

  if (strcmp(argv[3], "Target") == 0)
  {
    IMC::Target* tmsg = new IMC::Target;
    msg = tmsg;
    tmsg->label = "dune-sendmsg";
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->z = atof(argv[6]);

    if (argc > 7)
    {
      if (!strcmp(argv[7], "DEP") || !strcmp(argv[7], "dep"))
        tmsg->z_units = IMC::Z_DEPTH;
      else if (!strcmp(argv[7], "ALT") || !strcmp(argv[7], "alt"))
        tmsg->z_units = IMC::Z_ALTITUDE;
      else if (!strcmp(argv[7], "HEI") || !strcmp(argv[7], "hei"))
        tmsg->z_units = IMC::Z_HEIGHT;
    }

    if (argc > 9)
    {
      tmsg->cog = Angles::normalizeRadian(Angles::radians(atof(argv[8])));
      tmsg->sog = atof(argv[9]);
    }
  }

  if (strcmp(argv[3], "Temperature") == 0)
  {
    IMC::Temperature* tmsg = new IMC::Temperature;
    msg = tmsg;
    tmsg->value = atof(argv[4]);
  }

  if (strcmp(argv[3], "TeleoperationDone") == 0)
  {
    msg = new IMC::TeleoperationDone;
  }

  if (strcmp(argv[3], "TextMessage") == 0)
  {
    IMC::TextMessage* tmsg = new IMC::TextMessage;
    msg = tmsg;
    if (argc >= 6) {
      tmsg->origin = argv[4];
      tmsg->text = argv[5];
    }
    else if (argc == 5)
    {
      tmsg->origin = "dune-sendmsg";
      tmsg->text = argv[4];
    }
  }

  if (strcmp(argv[3], "TrexCommand") == 0)
  {
    IMC::TrexCommand* tmsg = new IMC::TrexCommand;
    msg = tmsg;
    if (strcmp(argv[4], "DISABLE") == 0 || strcmp(argv[4], "1") == 0 )
        tmsg->command = 1;
    else if (strcmp(argv[4], "ENABLE") == 0 || strcmp(argv[4], "2") == 0 )
        tmsg->command = 2;
  }

  if (strcmp(argv[3], "UASimulation") == 0)
  {
    IMC::UASimulation* tmsg = new IMC::UASimulation;
    tmsg->setSource(atoi(argv[4]));
    tmsg->setDestination(atoi(argv[5]));
    tmsg->speed = atoi(argv[6]);
    tmsg->type = IMC::UASimulation::UAS_DATA;
    tmsg->data.assign(atoi(argv[7]), '0');
    msg = tmsg;
  }

  if (strcmp(argv[3], "UsblConfig") == 0)
  {
    IMC::UsblConfig* tmsg = new IMC::UsblConfig;
    msg = tmsg;
    tmsg->op = IMC::UsblConfig::OP_SET_CFG;

    IMC::UsblModem modem;
    modem.name = argv[4];
    modem.lat = atof(argv[5]);
    modem.lon = atof(argv[6]);
    modem.z = atof(argv[7]);
    modem.z_units = static_cast<IMC::ZUnits>(1);
    tmsg->modems.push_back(modem);
  }

  if (strcmp(argv[3], "VehicleCommand") == 0)
  {
    IMC::VehicleCommand* tmsg = new IMC::VehicleCommand;
    msg = tmsg;
    tmsg->type = IMC::VehicleCommand::VC_REQUEST;
    tmsg->command = atoi(argv[4]);

    if (tmsg->command == IMC::VehicleCommand::VC_EXEC_MANEUVER)
      tmsg->maneuver.set(static_cast<IMC::Maneuver*>(IMC::Factory::produce(argv[5])));
  }

  if (strcmp(argv[3], "VehicleMedium") == 0)
  {
    IMC::VehicleMedium* tmsg = new IMC::VehicleMedium;
    msg = tmsg;
    tmsg->medium = atoi(argv[4]);
  }

  if (msg == NULL)
  {
    fprintf(stderr, "ERROR: unknown message '%s'\n", argv[3]);
    return 1;
  }

  msg->setTimeStamp();

  uint8_t bfr[1024] = {0};
  uint16_t rv = IMC::Packet::serialize(msg, bfr, sizeof(bfr));

  UDPSocket sock;
  try
  {
    sock.write(bfr, rv, dest, port);

    fprintf(stderr, "Raw:");
    for (int i = 0; i < rv; ++i)
      fprintf(stderr, " %02X", bfr[i]);
    fprintf(stderr, "\n");

    msg->toText(cerr);
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return 1;
  }

  if (msg != NULL)
  {
    delete msg;
    msg = NULL;
  }

  return 0;
}
Example #13
0
int
main(int argc, char** argv)
{
  if (argc < 4)
  {
    fprintf(stderr, "Usage: %s <destination host> <destination port> <abbrev> [arguments]\n", argv[0]);
    return 1;
  }

  Address dest(argv[1]);

  // Parse port.
  unsigned port = 0;
  if (!castLexical(argv[2], port))
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  if (port > 65535)
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  IMC::Message* msg = NULL;

  if (strcmp(argv[3], "Heartbeat") == 0)
  {
    IMC::Heartbeat* tmsg = new IMC::Heartbeat;
    msg = tmsg;
  }
  if (strcmp(argv[3], "RestartSystem") == 0)
  {
    IMC::RestartSystem* tmsg = new IMC::RestartSystem;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "Sms") == 0)
  {
    IMC::Sms* tmsg = new IMC::Sms;
    tmsg->number = argv[4];
    tmsg->timeout = atoi(argv[5]);
    tmsg->contents = argv[6];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "EntityState") == 0)
  {
    IMC::EntityState* tmsg = new IMC::EntityState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "EntityActivationState") == 0)
  {
    IMC::EntityActivationState* tmsg = new IMC::EntityActivationState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = IMC::EntityActivationState::EAS_ACTIVE;
  }
  else if (strcmp(argv[3], "MagneticField") == 0)
  {
    IMC::MagneticField* tmsg = new IMC::MagneticField;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->x = atof(argv[5]);
    tmsg->y = atof(argv[6]);
    tmsg->z = atof(argv[7]);
  }
  else if (strcmp(argv[3], "DataSanity") == 0)
  {
    IMC::DataSanity* tmsg = new IMC::DataSanity;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->sane = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "MonitorEntityState") == 0)
  {
    IMC::MonitorEntityState* tmsg = new IMC::MonitorEntityState;
    msg = tmsg;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
  }
  else if (strcmp(argv[3], "Abort") == 0)
  {
    msg = new IMC::Abort;
  }
  else if (strcmp(argv[3], "LoggingControl") == 0)
  {
    IMC::LoggingControl* tmsg = new IMC::LoggingControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
    tmsg->name = argv[5];
  }
  else if (strcmp(argv[3], "CacheControl") == 0)
  {
    IMC::CacheControl* tmsg = new IMC::CacheControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "LblRange") == 0)
  {
    IMC::LblRange* tmsg = new IMC::LblRange;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->range = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "LblConfig") == 0)
  {
    IMC::LblConfig* tmsg = new IMC::LblConfig;
    msg = tmsg;
    tmsg->op = IMC::LblConfig::OP_SET_CFG;

    IMC::LblBeacon bc;
    bc.beacon = "b2";
    bc.lat = 0.71883274;
    bc.lon = -0.15194732;
    bc.depth = 3;
    bc.query_channel = 4;
    bc.reply_channel = 5;
    bc.transponder_delay = 0;
    tmsg->beacons.push_back(bc);
    bc.beacon = "b3";
    bc.lat = 0.71881068;
    bc.lon = -0.15192335;
    bc.reply_channel = 6;
    tmsg->beacons.push_back(bc);
  }
  else if (strcmp(argv[3], "DesiredZ") == 0)
  {
    IMC::DesiredZ* tmsg = new IMC::DesiredZ;
    tmsg->value = atof(argv[4]);
    tmsg->z_units = static_cast<IMC::ZUnits>(atoi(argv[5]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredPitch") == 0)
  {
    IMC::DesiredPitch* tmsg = new IMC::DesiredPitch;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "Calibration") == 0)
  {
    IMC::Calibration* tmsg = new IMC::Calibration;
    tmsg->duration = (uint16_t)(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredHeading") == 0)
  {
    IMC::DesiredHeading* tmsg = new IMC::DesiredHeading;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredHeadingRate") == 0)
  {
    IMC::DesiredHeadingRate* tmsg = new IMC::DesiredHeadingRate;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredSpeed") == 0)
  {
    IMC::DesiredSpeed* tmsg = new IMC::DesiredSpeed;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->speed_units = IMC::SUNITS_PERCENTAGE;
    else
      tmsg->speed_units = atoi(argv[5]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredControl") == 0)
  {
    IMC::DesiredControl* tmsg = new IMC::DesiredControl;
    tmsg->k = atof(argv[4]);
    tmsg->m = atof(argv[5]);
    tmsg->n = atof(argv[6]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "SetThrusterActuation") == 0)
  {
    IMC::SetThrusterActuation* tmsg = new IMC::SetThrusterActuation;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }
  else if (strcmp(argv[3], "SetServoPosition") == 0)
  {
    IMC::SetServoPosition* tmsg = new IMC::SetServoPosition;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }
  else if (strcmp(argv[3], "GpsFix") == 0)
  {
    IMC::GpsFix* tmsg = new IMC::GpsFix;
    msg = tmsg;
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->height = atof(argv[6]);
  }
  else if (strcmp(argv[3], "VehicleCommand") == 0)
  {
    IMC::VehicleCommand* tmsg = new IMC::VehicleCommand;
    msg = tmsg;
    tmsg->type = IMC::VehicleCommand::VC_REQUEST;
    tmsg->command = atoi(argv[4]);

    if (tmsg->command == IMC::VehicleCommand::VC_EXEC_MANEUVER)
      tmsg->maneuver.set(dynamic_cast<IMC::Maneuver*>(IMC::Factory::produce(argv[5])));
  }
  else if (strcmp(argv[3], "ButtonEvent") == 0)
  {
    IMC::ButtonEvent* tmsg = new IMC::ButtonEvent;
    msg = tmsg;
    tmsg->button = atoi(argv[4]);
    tmsg->value = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "SetLedBrightness") == 0)
  {
    IMC::SetLedBrightness* tmsg = new IMC::SetLedBrightness;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->value = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "EstimatedState") == 0)
  {
    IMC::EstimatedState* tmsg = new IMC::EstimatedState;
    msg = tmsg;
    tmsg->x = atof(argv[4]);
    tmsg->y = atof(argv[5]);
    tmsg->z = atof(argv[6]);
    tmsg->phi = 0.0;
    tmsg->theta = 0.0;
    tmsg->psi = 0.0;
    tmsg->u = 0.0;
    tmsg->v = 0.0;
    tmsg->w = 0.0;
    tmsg->p = 0.0;
    tmsg->q = 0.0;
    tmsg->r = 0.0;
    tmsg->lat = 0.0;
    tmsg->lon = 0.0;
    tmsg->depth = 0.0;
  }
  else if (strcmp(argv[3], "PowerChannelControl") == 0)
  {
    IMC::PowerChannelControl* tmsg = new IMC::PowerChannelControl;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->op = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "AcousticSystemsQuery") == 0)
  {
    IMC::AcousticSystemsQuery* tmsg = new IMC::AcousticSystemsQuery;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "AcousticRange") == 0)
  {
    IMC::AcousticRange* tmsg = new IMC::AcousticRange;
    msg = tmsg;
    tmsg->address = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "AcousticMessage") == 0)
  {
    IMC::AcousticMessage* tmsg = new IMC::AcousticMessage;
    msg = tmsg;
    IMC::Message* imsg = IMC::Factory::produce(atoi(argv[4]));
    tmsg->message.set(*imsg);
    delete imsg;
  }
  else if (strcmp(argv[3], "AcousticPing") == 0)
  {
    msg = new IMC::AcousticPing;
  }
  else if (strcmp(argv[3], "QueryEntityInfo") == 0)
  {
    IMC::QueryEntityInfo* tmsg = new IMC::QueryEntityInfo;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "QueryEntityParameters") == 0)
  {
    IMC::QueryEntityParameters* tmsg = new IMC::QueryEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "SaveEntityParameters") == 0)
  {
    IMC::SaveEntityParameters* tmsg = new IMC::SaveEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "EntityList") == 0)
  {
    IMC::EntityList* tmsg = new IMC::EntityList;
    msg = tmsg;
    tmsg->op = IMC::EntityList::OP_QUERY;
  }
  else if (strcmp(argv[3], "ControlLoops") == 0)
  {
    IMC::ControlLoops* tmsg = new IMC::ControlLoops;
    msg = tmsg;
    tmsg->enable = atoi(argv[4]) ? 1 : 0;
    tmsg->mask = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "TeleoperationDone") == 0)
  {
    msg = new IMC::TeleoperationDone;
  }
  else if (strcmp(argv[3], "RemoteActionsRequest") == 0)
  {
    IMC::RemoteActionsRequest* tmsg = new IMC::RemoteActionsRequest;
    msg = tmsg;
    tmsg->op = IMC::RemoteActionsRequest::OP_QUERY;
  }
  else if (strcmp(argv[3], "RemoteActions") == 0)
  {
    IMC::RemoteActions* tmsg = new IMC::RemoteActions;
    msg = tmsg;
    tmsg->actions = argv[4];
  }
  else if (strcmp(argv[3], "LogBookControl") == 0)
  {
    IMC::LogBookControl* tmsg = new IMC::LogBookControl;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->htime = Time::Clock::getSinceEpoch() - atof(argv[5]);
    else
      tmsg->htime = -1;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "EmergencyControl") == 0)
  {
    IMC::EmergencyControl* tmsg = new IMC::EmergencyControl;
    tmsg->command = atoi(argv[4]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "LeakSimulation") == 0)
  {
    IMC::LeakSimulation* tmsg = new IMC::LeakSimulation;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "OperationalLimits") == 0)
  {
    IMC::OperationalLimits* tmsg = new IMC::OperationalLimits;
    tmsg->mask = IMC::OPL_AREA;
    tmsg->lat = DUNE::Math::Angles::radians(atof(argv[4]));
    tmsg->lon = DUNE::Math::Angles::radians(atof(argv[5]));
    tmsg->orientation = DUNE::Math::Angles::radians(atof(argv[6]));
    tmsg->width = atof(argv[7]);
    tmsg->length = atof(argv[8]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "UASimulation") == 0)
  {
    IMC::UASimulation* tmsg = new IMC::UASimulation;
    tmsg->setSource(atoi(argv[4]));
    tmsg->setDestination(atoi(argv[5]));
    tmsg->speed = atoi(argv[6]);
    tmsg->type = IMC::UASimulation::UAS_DATA;
    tmsg->data.assign(atoi(argv[7]), '0');
    msg = tmsg;
  }
  else if (strcmp(argv[3], "ReplayControl") == 0)
  {
    IMC::ReplayControl* tmsg = new IMC::ReplayControl;
    tmsg->op = atoi(argv[4]);
    if (tmsg->op == IMC::ReplayControl::ROP_START)
      tmsg->file = argv[5];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "ClockControl") == 0)
  {
    IMC::ClockControl* tmsg = new IMC::ClockControl;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->clock = atof(argv[5]);
    if (argc >= 7)
      tmsg->tz = atoi(argv[6]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "PlanControl") == 0)
  {
    IMC::PlanControl* tmsg = new IMC::PlanControl;
    tmsg->type = IMC::PlanControl::PC_REQUEST;
    tmsg->op = atoi(argv[4]);
    tmsg->plan_id = argv[5];
    if (argc >= 7)
      tmsg->flags = atoi(argv[6]);

    if (argc >= 8)
      tmsg->arg.set(IMC::Factory::produce(argv[7]));

    msg = tmsg;
  }
  else if (strcmp(argv[3], "LogBookEntry") == 0)
  {
    IMC::LogBookEntry* tmsg = new IMC::LogBookEntry;
    msg = tmsg;
    tmsg->context = argv[4];
    tmsg->text = argv[5];
    tmsg->htime = Time::Clock::getSinceEpoch();
    if (argc > 6)
      tmsg->type = atoi(argv[6]);
    else
      tmsg->type = IMC::LogBookEntry::LBET_WARNING;
  }
  else if (strcmp(argv[3], "TrexCommand") == 0)
  {
    IMC::TrexCommand* tmsg = new IMC::TrexCommand;
    msg = tmsg;
    if (strcmp(argv[4], "DISABLE") == 0 || strcmp(argv[4], "1") == 0 )
        tmsg->command = 1;
    else if (strcmp(argv[4], "ENABLE") == 0 || strcmp(argv[4], "2") == 0 )
        tmsg->command = 2;
  }
  else if (strcmp(argv[3], "PlanGeneration") == 0)
  {
    IMC::PlanGeneration* tmsg = new IMC::PlanGeneration;
    msg = tmsg;
    tmsg->cmd = atoi(argv[4]);
    tmsg->op = atoi(argv[5]);
    tmsg->plan_id = argv[6];

    if (argc >= 8)
      tmsg->params = argv[7];
  }
  else if (strcmp(argv[3], "SoundSpeed") == 0)
  {
    IMC::SoundSpeed* tmsg = new IMC::SoundSpeed;
    msg = tmsg;
    tmsg->value = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "Parameter") == 0)
  {
    IMC::Parameter* tmsg = new IMC::Parameter;
    msg = tmsg;
    tmsg->section = argv[4];
    tmsg->param = argv[5];
    tmsg->value = argv[6];
  }
  else if (strcmp(argv[3], "DevCalibrationControl") == 0)
  {
    IMC::DevCalibrationControl * tmsg = new IMC::DevCalibrationControl;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->op = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "RegisterManeuver") == 0)
  {
    IMC::RegisterManeuver* tmsg = new IMC::RegisterManeuver;
    msg = tmsg;
    tmsg->mid = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "Brake") == 0)
  {
    IMC::Brake* tmsg = new IMC::Brake;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "SetEntityParameters") == 0)
  {
    IMC::SetEntityParameters* tmsg = new IMC::SetEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];

    IMC::EntityParameter param;

    unsigned i = 4;

    while (1)
    {
      if (argc >= (int)i + 2)
      {
        ++i;
        param.name = argv[i];
        ++i;
        param.value = argv[i];

        tmsg->params.push_back(param);
      }
      else
      {
        break;
      }
    }
  }
  else if (strcmp(argv[3], "PushEntityParameters") == 0)
  {
    IMC::PushEntityParameters* tmsg = new IMC::PushEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "PopEntityParameters") == 0)
  {
    IMC::PopEntityParameters* tmsg = new IMC::PopEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "IridiumMsgTx") == 0)
  {
    IMC::IridiumMsgTx* tmsg = new IMC::IridiumMsgTx;
    msg = tmsg;
    tmsg->req_id = atoi(argv[4]);
    tmsg->ttl = atoi(argv[5]);
    std::string hex = String::fromHex(argv[6]);
    tmsg->data.assign(hex.begin(), hex.end());
  }

  if (msg == NULL)
  {
    fprintf(stderr, "ERROR: unknown message '%s'\n", argv[3]);
    return 1;
  }

  msg->setTimeStamp();

  uint8_t bfr[1024] = {0};
  uint16_t rv = IMC::Packet::serialize(msg, bfr, sizeof(bfr));

  UDPSocket sock;
  try
  {
    sock.write((const char*)bfr, rv, dest, port);

    fprintf(stderr, "Raw:");
    for (int i = 0; i < rv; ++i)
      fprintf(stderr, " %02X", bfr[i]);
    fprintf(stderr, "\n");

    msg->toText(cerr);
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return 1;
  }

  if (msg != NULL)
  {
    delete msg;
    msg = NULL;
  }

  return 0;
}
int main(int argc, char* argv[]) {

    //parse args
    if (argc != 5) {
        std::cout << "This program requires 4 arguments in the following format!" << std::endl;
        std::cout << "sender <host address of emulator> <udp port number of emulator in FWD direction>" << "\n"
                  << "<udp port number of sender to receive acks> <fileName>" << std::endl;
        std::cout << "Example: sender 192.168.89.78 49998 11234 test.txt" << std::endl;
        return 1;
    }


    const char* ipAddr = argv[1];
    if (0 == strcmp(ipAddr, "localhost")) {
        ipAddr = "127.0.0.1";
    }

    unsigned long remoteIp = inet_addr(ipAddr);
    int remotePort = atoi(argv[2]);
    int senderPort = atoi(argv[3]);
    const char* fileName = argv[4];

    int totalPackets = 0;
    int totalBytes = 0;

    int sentPackets = 0;
    int sentBytes = 0;

    int seqNum = 0;
    signal(SIGALRM, signal_alarm_handler);
    bool timer_on = false;
    list<Packet *> packetsToSend;
    list<Packet *> packetsSent;

    //Open the file and prepare the packets for transmission
    ifstream transferFile(fileName);
    ofstream seqLog(seqFile);
    ofstream ackLog(ackFile);

    if (!(seqLog.is_open() && ackLog.is_open())) {
        return 1;
    }

    if (transferFile.is_open()) {
        char buffer[Packet::MAX_DATA_LENGTH];

        while(false == transferFile.eof()) {
            transferFile.read(buffer, Packet::MAX_DATA_LENGTH);
            Packet* packet = Packet::createPacket(seqNum, transferFile.gcount(), buffer);
            packetsToSend.insert(packetsToSend.end(), packet);

            LOG("Inserted packet %i in the queue\n", totalPackets);

            seqNum++;
            seqNum %= Packet::MAX_SEQ_NUMBER;
            totalPackets++;
            totalBytes += packet->getLength();
        }
    } else {
        LOG("The input file was not found..\n");
        return 1;
    }

    transferFile.close();

    //Add EOT packet
    Packet *eotPacket = Packet::createEOT(seqNum);
    packetsToSend.insert(packetsToSend.end(), eotPacket);
    totalPackets++;

    LOG("Packets ready for transmission: %i\n", totalPackets);
    LOG("Total data to be sent: %i\n", totalBytes);

    //open a udp socket and listen for str
    UDPSocket udpSocket;
    int ret = udpSocket.open(senderPort);
    ASSERT(ret == 0);
    LOG("Sender is transmitting & receiving on UDP Port %i\n", senderPort);

    /**
    * Core Logic
    * The sender can be in three states
    * 1. SEND_DATA : Sends a packet as long as data is available and window size is permitted
    * 2. RECV_ACK : Receives an acknowledgement for a previously sent data
    * 3. RESEND_DATA: If an ACK is not received for a previously sent data within timer, resend the data
    */
    while ((packetsToSend.size() > 0) || (packetsSent.size() > 0)) {
        if (udpSocket.hasData()) {
            g_state = SENDER_RECV_ACK;
        }

        switch(g_state)
        {
        /**
        * Within the WINDOW_SIZE, if data is available, send it.
        * Remove it from packetsToSend, and add it to packetsSent
        * If timer is off, turn it on.
        */
        case SENDER_SEND_DATA: {
            if (packetsSent.size() < WINDOW_SIZE && packetsToSend.size() > 0) {
                list<Packet *>::iterator itr = packetsToSend.begin();
                Packet *packet = (*itr);
                char *packetData = packet->getUDPData();
                udpSocket.write(remoteIp, remotePort, packetData, packet->getUDPSize());
                sentPackets++;
                sentBytes += packet->getLength();

                seqLog << packet->getSeqNum() << endl;
                LOG("Sent packet seqnum %i, packet number %i\n", packet->getSeqNum(), sentPackets);
                LOG("Sent bytes: %i\n", sentBytes);

                if (false == timer_on) {
                    alarm(TIMER_SECS);
                    timer_on = true;
                }

                free(packetData);

                packetsToSend.remove(packet);
                packetsSent.insert(packetsSent.end(), packet);
            }
        }
        break;

        /**
        * 1. RECV Packet.
        * 2. Check if the ack number belongs to any packets in packetsSent.
        * 3. If yes, delete all packets upto and before.
        * 4. If not, discard packet
        * 5. If any packets are sent and not recieved ack, start timer again.
        */
        case SENDER_RECV_ACK: {
            char buffer[Packet::MAX_PACKET_LENGTH];
            unsigned long ip;
            int port;
            int length = Packet::MAX_PACKET_LENGTH;
            udpSocket.read(buffer, length, ip, port);

            Packet *packet = Packet::parseUDPData(buffer);
            int seqNum = packet->getSeqNum();
            ackLog << seqNum << endl;

            //since it is cumulative keep deleting until seq num is found on packetsSent
            list<Packet *>::iterator itr = packetsSent.begin();
            while (itr != packetsSent.end() && comparePacket((*itr)->getSeqNum(), seqNum) <= 0) { //less than or equal
                Packet *sentPacket = (*itr);
                packetsSent.remove(sentPacket);
                itr = packetsSent.begin();

                delete(sentPacket);
            }

            delete(packet);

            if (packetsSent.size() > 0) {
                alarm(TIMER_SECS);
            }

            g_state = SENDER_SEND_DATA;

        }
        break;

        /**
        * 1. Resend data if timeout.
        * 2. Send all packets in packetsToSend
        */
        case SENDER_RESEND_DATA: {
            for (list<Packet *>::iterator itr = packetsSent.begin(); itr != packetsSent.end(); ++itr) {
                Packet *packet = (*itr);
                char *packetData = packet->getUDPData();
                udpSocket.write(remoteIp, remotePort, packetData, packet->getUDPSize());

                seqLog << packet->getSeqNum() << endl;
                LOG("Resent packet seqnum %i\n", packet->getSeqNum());

                if (false == timer_on) {
                    alarm(TIMER_SECS);
                    timer_on = true;
                }

                free(packetData);
            }
        }
        break;

        default: {
            ASSERT(0); //invalid state
        }
        }
    }

    LOG("End of Transmission. Exiting \n");
    udpSocket.close();
    seqLog.close();
    ackLog.close();

    ASSERT(totalBytes == sentBytes);
    ASSERT(totalPackets == sentPackets);

    return 0;
}
void gWriteGSMTAP(unsigned ARFCN, unsigned TS, unsigned FN,
                  GSM::TypeAndOffset to, bool is_saach, bool ul_dln,
                  const BitVector& frame)
{
	char buffer[MAX_UDP_LENGTH];
	int ofs = 0;

	// Check if GSMTap is enabled
	if (!gConfig.defines("GSMTAP.TargetIP")) return;

	// Port configuration
	unsigned port = GSMTAP_UDP_PORT;	// default port for GSM-TAP
	if (gConfig.defines("GSMTAP.TargetPort"))
		port = gConfig.getNum("GSMTAP.TargetPort");

	// Set socket destination
	GSMTAPSocket.destination(port,gConfig.getStr("GSMTAP.TargetIP").c_str());

	// Decode TypeAndOffset
	uint8_t stype, scn;

	switch (to) {
		case GSM::TDMA_BEACON_BCCH:
			stype = GSMTAP_CHANNEL_BCCH;
			scn = 0;
			break;

		case GSM::TDMA_BEACON_CCCH:
			stype = GSMTAP_CHANNEL_CCCH;
			scn = 0;
			break;

		case GSM::SDCCH_4_0:
		case GSM::SDCCH_4_1:
		case GSM::SDCCH_4_2:
		case GSM::SDCCH_4_3:
			stype = GSMTAP_CHANNEL_SDCCH4;
			scn = to - GSM::SDCCH_4_0;
			break;

		case GSM::SDCCH_8_0:
		case GSM::SDCCH_8_1:
		case GSM::SDCCH_8_2:
		case GSM::SDCCH_8_3:
		case GSM::SDCCH_8_4:
		case GSM::SDCCH_8_5:
		case GSM::SDCCH_8_6:
		case GSM::SDCCH_8_7:
			stype = GSMTAP_CHANNEL_SDCCH8;
			scn = to - GSM::SDCCH_8_0;
			break;

		case GSM::TCHF_0:
			stype = GSMTAP_CHANNEL_TCH_F;
			scn = 0;
			break;

		case GSM::TCHH_0:
		case GSM::TCHH_1:
			stype = GSMTAP_CHANNEL_TCH_H;
			scn = to - GSM::TCHH_0;
			break;

		default:
			stype = GSMTAP_CHANNEL_UNKNOWN;
			scn = 0;
	}

	if (is_saach)
		stype |= GSMTAP_CHANNEL_ACCH;

	// Flags in ARFCN
	if (gConfig.getNum("GSM.Band") == 1900)
		ARFCN |= GSMTAP_ARFCN_F_PCS;

	if (ul_dln)
		ARFCN |= GSMTAP_ARFCN_F_UPLINK;

	// Build header
	struct gsmtap_hdr *header = (struct gsmtap_hdr *)buffer;
	header->version			= GSMTAP_VERSION;
	header->hdr_len			= sizeof(struct gsmtap_hdr) >> 2;
	header->type			= GSMTAP_TYPE_UM;
	header->timeslot		= TS;
	header->arfcn			= htons(ARFCN);
	header->signal_dbm		= 0; /* FIXME */
	header->snr_db			= 0; /* FIXME */
	header->frame_number	= htonl(FN);
	header->sub_type		= stype;
	header->antenna_nr		= 0;
	header->sub_slot		= scn;
	header->res				= 0;

	ofs += sizeof(*header);

	// Add frame data
	frame.pack((unsigned char*)&buffer[ofs]);
	ofs += (frame.size() + 7) >> 3;

	// Write the GSMTAP packet
	GSMTAPSocket.write(buffer, ofs);
}
Example #16
0
int
main(int argc, char** argv)
{
  double speed = 1, begin = 0, end = -1;
  std::map<std::string, bool> filter;
  bool filtering = false;
  int verbose = 0;
  uint16_t src = 0xFFFF, dst = 0xFFFF;

  ++argv; --argc;

  if (!argc)
  {
    usage();
    return 1;
  }

  for (; *argv && **argv == '-'; ++argv, --argc)
  {
    char opt = (*argv)[1];
    ++argv; --argc;

    if (!*argv || **argv == '-')
    {
      std::cerr << "Invalid options\n";
      usage();
      return 1;
    }

    // @todo Use DUNE's OptionParser, too lazy now to do it.
    switch (opt)
    {
      case 'b':
      {
        char* aux;
        begin = std::strtod(*argv, &aux);
        if (*aux != 0 || begin < 0)
        {
          std::cerr << "Invalid begin time: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'e':
      {
        char* aux;
        end = std::strtod(*argv, &aux);
        if (*aux != 0 || end < 0)
        {
          std::cerr << "Invalid end time: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }

      case 'S':
      {
        char* aux;
        src = std::strtol(*argv, &aux, 10);
        if (*aux != 0)
        {
          std::cerr << "Invalid source address: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'D':
      {
        char* aux;
        dst = std::strtol(*argv, &aux, 10);
        if (*aux != 0)
        {
          std::cerr << "Invalid destination adress: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 's':
      {
        char* aux;
        speed = std::strtod(*argv, &aux);
        if (*aux != 0 || speed < 0)
        {
          std::cerr << "Invalid speed setting: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'v':
        verbose = std::atoi(*argv);
        break;
      case 'm':
      {
        std::vector<std::string> list;
        DUNE::Utils::String::split(*argv, ",", list);
        for (uint16_t i = 0; i < list.size(); ++i)
          filter[list[i]] = true;
        filtering = true;
      }
      break;
      default:
        std::cerr << "Invalid option: '-" << opt << "\'\n";
        usage();
        return 1;
    }
  }

  if (argc < 3)
  {
    std::cerr << "Invalid arguments" << std::endl;
    usage();
    return 1;
  }

  if (begin > 0 && end > 0 && begin > end)
  {
    std::cerr << "Invalid time offsets" << std::endl;
    usage();
    return 1;
  }

  UDPSocket sock;
  Address dest(argv[0]);
  uint16_t port = std::atoi(argv[1]);

  argv += 2;

  std::cout << std::fixed << std::setprecision(4);

  for (; *argv != 0; argv++)
  {
    Path file(*argv);
    std::istream* is;

    if (file.isDirectory())
    {
      file = file / "Data.lsf";
      if (!file.isFile())
        file += ".gz";
    }

    if (!file.isFile())
    {
      std::cerr << file << " does not exist\n";
      return 1;
    }

    Compression::Methods method = Compression::Factory::detect(file.c_str());
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(file.c_str(), std::ios::binary);
    else
      is = new Compression::FileInput(file.c_str(), method);

    IMC::Message* m;

    m = IMC::Packet::deserialize(*is);
    if (!m)
    {
      std::cerr << file << " contains no messages\n";
      delete is;
      continue;
    }

    DUNE::Utils::ByteBuffer bb;

    double time_origin = m->getTimeStamp();
    if (begin >= 0)
    {
      do
      {
        if (m->getTimeStamp() - time_origin >= begin)
          break;
        delete m;
        m = IMC::Packet::deserialize(*is);
      }
      while (m);

      if (!m)
      {
        std::cerr << "no messages for specified time range" << std::endl;
        return 1;
      }
    }
    else
      begin = 0;

    double start_time = Clock::getSinceEpoch();
    double now = start_time;

    do
    {
      double msg_ts = m->getTimeStamp();
      double vtime = msg_ts - time_origin;

      m->setTimeStamp(start_time + vtime);

      double future = 0;

      if (speed > 0 && vtime >= begin)
      {
        // Delay time to mimic behavior at specified speed
        future = start_time + vtime / speed - begin;
        double delay_time = (future - now);
        if (delay_time > 0)
          Delay::wait(delay_time);
      }
      now = Clock::getSinceEpoch();

      if (vtime >= begin
          && (src == 0xFFFF || src == m->getSource())
          && (dst == 0xFFFF || dst == m->getDestination())
          && (!filtering || filter[m->getName()]))
      {
        // Send message
        IMC::Packet::serialize(m, bb);
        sock.write(bb.getBuffer(), m->getSerializationSize(), dest, port);
        if (verbose >= 1)
          std::cout << (begin + now - start_time) << ' ' << vtime << ' ' << now - future << " : " << m->getName() << '\n';
        if (verbose >= 2)
          m->toText(std::cout);
      }

      delete m;

      if (end >= 0 && vtime >= end)
        break;
    }
    while ((m = IMC::Packet::deserialize(*is)) != 0);
    delete is;
  }
  return 0;
}