void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::FollowPath& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
int n_points;
cfg.get(id, "Number of Points", "0", n_points);
Math::Matrix W(n_points, 3);
W.readFromConfig(cfg, id, "Points");
IMC::MessageList<IMC::PathPoint>* list = &man.points;
for (int i = 0; i < W.rows(); ++i)
{
IMC::PathPoint* p = new IMC::PathPoint;
p->x = W(i, 0);
p->y = W(i, 1);
p->z = W(i, 2);
list->push_back(*p);
}
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Goto& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Dubin& man)
{
// Get configurable parameters
parseSpeed(cfg, id, man);
parseDuration(cfg, id, man);
parseTimeout(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::PopUp& man)
{
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
cfg.get(id, "Radius (meters)", "15.0", man.radius);
cfg.get(id, "Flags", "0x00", man.flags);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Loiter& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
int8_t type;
cfg.get(id, "Loiter Type", "0", type);
switch (type)
{
case 1:
man.type = IMC::Loiter::LT_RACETRACK; break;
case 2:
man.type = IMC::Loiter::LT_HOVER; break;
case 3:
man.type = IMC::Loiter::LT_EIGHT; break;
case 0:
default:
man.type = IMC::Loiter::LT_CIRCULAR; break;
}
std::string ldir;
cfg.get(id, "Loiter Direction", "Clock", ldir);
if (ldir == "Clockwise")
man.direction = IMC::Loiter::LD_CLOCKW;
else
man.direction = IMC::Loiter::LD_CCLOCKW;
cfg.get(id, "Radius (meters)", "50", man.radius);
parseAngle(cfg, id, "Bearing (degrees)", man.bearing, 0.0);
cfg.get(id, "Length (meters)", "100", man.length);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::CompassCalibration& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
std::string ldir;
cfg.get(id, "Loiter Direction", "Clock", ldir);
if (ldir == "Clockwise")
man.direction = IMC::Loiter::LD_CLOCKW;
else
man.direction = IMC::Loiter::LD_CCLOCKW;
cfg.get(id, "Radius (meters)", "50", man.radius);
cfg.get(id, "Amplitude (meters)", "1", man.amplitude);
parseAngle(cfg, id, "Pitch (degrees)", man.pitch, (fp32_t)0.0);
}
signed char argumentManagement( long l_argc, char **ppch_argv, opt_t *pstr_argsDest )
{
signed char c_retValue;
long l_opt, l_optIndex;
static struct option tstr_longOpt[] = {
{ "interface", required_argument, 0, 'I' },
{ "count", required_argument, 0, 'c' },
{ "send-replies", required_argument, 0, 'a' },
{ "timeout", required_argument, 0, 't' },
{ "delay", required_argument, 0, 'w' },
{ "retries", required_argument, 0, 'r' },
{ "source-ip", required_argument, 0, 's' },
{ "exit-on-reply", no_argument, 0, 'q' },
{ "version", no_argument, 0, 'V' },
{ "help", no_argument, 0, 'h' },
{ 0, 0, 0, 0 }
};
/* Initialisation */
c_retValue = 1;
l_optIndex = 0;
initOptionsDefault( pstr_argsDest );
/* ************** */
/* External variables init */
optarg = NULL;
optind = 1;
opterr = 1; /* Set to zero to inhibit error messages on unrecognized options */
optopt = '?'; /* Char to return on unrecognized option */
/* Parsing options args */
while ( ( l_opt = getopt_long( l_argc, ppch_argv, "I:c:t:a:w:r:s:qVh", tstr_longOpt, ( int * )&l_optIndex ) ) != -1 )
{
switch( l_opt )
{
/* Interface to use */
case 'I' : pstr_argsDest->pch_iface = optarg;
break;
/* number of packets to send (infinite if nothing specified */
case 'c' : pstr_argsDest->ul_count = ABS( atol( optarg ) ); /* < 0 were stupid */
if ( pstr_argsDest->ul_count == 0 )
c_retValue = ERR_ARG_PARSING;
break;
/* Send RARP replies instead of requests */
case 'a' : pstr_argsDest->uc_choosenOpCode = RARP_OPCODE_REPLY;
pstr_argsDest->pch_IpAddrRarpReplies = optarg;/* content of the reply (IP address) */
break;
/* set timeout */
case 't' : parseTimeout( &( pstr_argsDest->str_timeout ), optarg );
break;
/* pause between probes */
case 'w' : pstr_argsDest->ul_waitingMilliSeconds = 0;
pstr_argsDest->ul_waitingMilliSeconds = ABS( atol( optarg ) );
if ( pstr_argsDest->ul_waitingMilliSeconds == 0 )
{
fprintf( stderr, "Ivalid delay (%s), must be an integer, of milliseconds\n", optarg );
exit( EXIT_FAILURE );
}
break;
/* retries on unanswered probes */
case 'r' : pstr_argsDest->uc_unlimitedRetries = 0;
pstr_argsDest->ul_maximumRetries = ABS( atol( optarg ) ); /* if incorrect => atol send us zero, this will be good too */
break;
/* spoof local IP address */
case 's' : pstr_argsDest->pch_spoofedLocalIpAddress = optarg;
break;
/* exit on first catched reply */
case 'q' : pstr_argsDest->uc_exitOnReply = 1;
break;
/* print version and exit */
case 'V' : fprintf( stdout, "%s\n", VERSION );
exit( EXIT_FAILURE );
break;
/* print out a short help mesage */
case 'h' :
case '?' :
default : c_retValue = ERR_ARG_PARSING;
}
}
/* parsing non options args */
/* The only one must be the MAC Addr we'll request related IP */
if ( optind < l_argc )
pstr_argsDest->pch_askedHwAddr = ppch_argv[optind];
else
c_retValue = ERR_ARG_PARSING;
/* Check if required infos had been given */
if ( ( pstr_argsDest->pch_iface == NULL ) || ( pstr_argsDest->pch_askedHwAddr == NULL ) )
c_retValue = ERR_ARG_PARSING;
else
{
fprintf( stdout, "RARPING %s on %s\n", pstr_argsDest->pch_askedHwAddr, pstr_argsDest->pch_iface );
}
return c_retValue;
}