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 section, IMC::YoYo& man)
{
parseCoordinate(cfg, section, man);
parseZ(cfg, section, man);
parseZUnits(cfg, section, man);
cfg.get(section, "Amplitude (meters)", "0.0", man.amplitude);
parseAngle(cfg, section, "Pitch (degrees)", man.pitch, (fp32_t)15.0);
parseSpeed(cfg, section, man);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::StationKeeping& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
parseDuration(cfg, id, man);
cfg.get(id, "Radius (meters)", "15.0", man.radius);
}
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 section, IMC::Elevator& man)
{
parseSpeed(cfg, section, man);
parseCoordinate(cfg, section, man);
cfg.get(section, "Flags", "0x00", man.flags);
parseZUnits(cfg, section, man.start_z_units, "Start Z Units");
parseZUnits(cfg, section, man.end_z_units, "End Z Units");
cfg.get(section, "Start Z (meters)", "0.0", man.start_z);
cfg.get(section, "End Z (meters)", "0.0", man.end_z);
cfg.get(section, "Radius (meters)", "15.0", man.radius);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Rows& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
parseAngle(cfg, id, "Bearing (degrees)", man.bearing, 0.0);
parseAngle(cfg, id, "Cross Angle (degrees)", man.cross_angle, 0.0);
cfg.get(id, "Width (meters)", "150", man.width);
cfg.get(id, "Length (meters)", "100", man.length);
cfg.get(id, "Curve Offset (meters)", "15", man.coff);
cfg.get(id, "Alternation (%)", "100", man.alternation);
cfg.get(id, "Horizontal Step (meters)", "30", man.hstep);
cfg.get(id, "Flags", "3", man.flags);
}
bool ubloxNMEA::parsePUBX_00( char chr )
{
bool ok = true;
switch (fieldIndex) {
case 1:
// The first field is actually a message subtype
if (chrCount == 0)
ok = (chr == '0');
else if (chrCount == 1) {
switch (chr) {
#if defined(NMEAGPS_PARSE_PUBX_00) | defined(NMEAGPS_RECOGNIZE_ALL)
case '0': break;
#endif
#if defined(NMEAGPS_PARSE_PUBX_04) | defined(NMEAGPS_RECOGNIZE_ALL)
case '4': nmeaMessage = (nmea_msg_t) PUBX_04; break;
#endif
default : ok = false;
}
} else // chrCount > 1
ok = (chr == ',');
break;
#ifdef NMEAGPS_PARSE_PUBX_00
case 2: return parseTime( chr );
PARSE_LOC(3);
case 7: return parseAlt( chr );
case 8: return parseFix( chr );
case 11: return parseSpeed( chr ); // kph!
case 12: return parseHeading( chr );
case 15: return parseHDOP( chr );
case 16: return parseVDOP( chr );
case 18: return parseSatellites( chr );
#endif
}
return ok;
} // parsePUBX_00
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);
}
bool ubloxNMEA::parsePUBX_00( char chr )
{
bool ok = true;
switch (fieldIndex) {
case 1:
// The first field is actually a message subtype
if (chrCount == 0)
ok = (chr == '0');
else if (chrCount == 1) {
switch (chr) {
#if defined(NMEAGPS_PARSE_PUBX_00) | defined(NMEAGPS_RECOGNIZE_ALL)
case '0': break;
#endif
#if defined(NMEAGPS_PARSE_PUBX_04) | defined(NMEAGPS_RECOGNIZE_ALL)
case '4': nmeaMessage = (nmea_msg_t) PUBX_04; break;
#endif
default : ok = false;
}
} else // chrCount > 1
ok = (chr == ',');
break;
#ifdef NMEAGPS_PARSE_PUBX_00
case 2: return parseTime( chr );
PARSE_LOC(3);
case 7: return parseAlt( chr );
case 8: return parseFix( chr );
case 9: // use Horizontal accuracy for both lat and lon errors
#if defined(GPS_FIX_LAT_ERR)
ok = parse_lat_err( chr );
#if defined(GPS_FIX_LON_ERR)
// When the lat_err field is finished,
// copy it to the lon_err field.
if (chr == ',') {
m_fix.valid.lon_err = m_fix.valid.lat_err;
if (m_fix.valid.lon_err)
m_fix.lon_err_cm = m_fix.lat_err_cm;
}
#endif
#elif defined(GPS_FIX_LON_ERR)
ok = parse_lon_err( chr );
#endif
break;
case 10: return parse_alt_err( chr ); // vertical accuracy
case 11:
#ifdef GPS_FIX_SPEED
ok = parseSpeed( chr ); // PUBX,00 provides speed in km/h!
if ((chr == ',') && m_fix.valid.speed) {
uint32_t kph = m_fix.spd.int32_000();
uint32_t nmiph = (kph * 1000) / gps_fix::M_PER_NMI;
m_fix.spd.whole = nmiph / 1000;
m_fix.spd.frac = (nmiph - m_fix.spd.whole*1000);
// Convert to Nautical Miles/Hour
}
#endif
break;
case 12: return parseHeading( chr );
case 13: return parseVelocityDown( chr );
case 15: return parseHDOP( chr );
case 16: return parseVDOP( chr );
case 18: return parseSatellites( chr );
#endif
}
return ok;
} // parsePUBX_00
