void WG06::diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
stringstream str;
str << "Accelerometer (" << actuator_info_.name_ << ")";
d.name = str.str();
char serial[32];
snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
d.hardware_id = serial;
d.summary(d.OK, "OK");
d.clear();
pr2_hardware_interface::AccelerometerCommand acmd(accelerometer_.command_);
const char * range_str =
(acmd.range_ == 0) ? "+/-2G" :
(acmd.range_ == 1) ? "+/-4G" :
(acmd.range_ == 2) ? "+/-8G" :
"INVALID";
const char * bandwidth_str =
(acmd.bandwidth_ == 6) ? "1500Hz" :
(acmd.bandwidth_ == 5) ? "750Hz" :
(acmd.bandwidth_ == 4) ? "375Hz" :
(acmd.bandwidth_ == 3) ? "190Hz" :
(acmd.bandwidth_ == 2) ? "100Hz" :
(acmd.bandwidth_ == 1) ? "50Hz" :
(acmd.bandwidth_ == 0) ? "25Hz" :
"INVALID";
// Board revB=1 and revA=0 does not have accelerometer
bool has_accelerometer = (board_major_ >= 2);
double sample_frequency = 0.0;
ros::Time current_time(ros::Time::now());
if (!first_publish_)
{
sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
{
if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
{
d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
}
}
}
accelerometer_samples_ = 0;
d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
d.addf("Accelerometer sample frequency", "%f", sample_frequency);
d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);
}
/* --- parse the commandline and build shell commmand structure --- */
int parsecommand(char *cmdline, Shellcmd *shellcmd)
{
int i, n;
Cmd *cmd0;
char *t = cmdline;
char *tok;
// Initialize list
for (i = 0; i < COMMANDMAX-1; i++) cmdbuf[i].next = &cmdbuf[i+1];
cmdbuf[COMMANDMAX-1].next = NULL;
cmds = cmdbuf;
cp = cbuf;
pp = pbuf;
shellcmd->rd_stdin = NULL;
shellcmd->rd_stdout = NULL;
shellcmd->rd_stderr = NULL;
shellcmd->background = 0; // false
shellcmd->the_cmds = NULL;
do {
if ((n = acmd(t, &cmd0)) <= 0)
return -1;
t += n;
cmd0->next = shellcmd->the_cmds;
shellcmd->the_cmds = cmd0;
int newtoken = 1;
while (newtoken) {
n = nexttoken(t, &tok);
if (n == 0)
{
return 1;
}
t += n;
switch(*tok) {
case PIPE:
newtoken = 0;
break;
case BG:
n = nexttoken(t, &tok);
if (n == 0)
{
shellcmd->background = 1;
return 1;
}
else
{
fprintf(stderr, "illegal bakgrounding\n");
return -1;
}
newtoken = 0;
break;
case RIN:
if (shellcmd->rd_stdin != NULL)
{
fprintf(stderr, "duplicate redirection of stdin\n");
return -1;
}
if ((n = nexttoken(t, &(shellcmd->rd_stdin))) < 0)
return -1;
if (!isidentifier(shellcmd->rd_stdin))
{
fprintf(stderr, "Illegal filename: \"%s\"\n", shellcmd->rd_stdin);
return -1;
}
t += n;
break;
case RUT:
if (shellcmd->rd_stdout != NULL)
{
fprintf(stderr, "duplicate redirection of stdout\n");
return -1;
}
if ((n = nexttoken(t, &(shellcmd->rd_stdout))) < 0)
return -1;
if (!isidentifier(shellcmd->rd_stdout))
{
fprintf(stderr, "Illegal filename: \"%s\"\n", shellcmd->rd_stdout);
return -1;
}
t += n;
break;
default:
return -1;
}
}
} while (1);
return 0;
}
int
parse (char *buf, Command *c)
{
int n;
Pgm *cmd0;
char *t = buf;
char *tok;
init();
c->rstdin = NULL;
c->rstdout = NULL;
c->rstderr = NULL;
c->bakground = 0; /* false */
c->pgm = NULL;
newcmd:
if ((n = acmd(t, &cmd0)) <= 0)
return -1;
t += n;
cmd0->next = c->pgm;
c->pgm = cmd0;
newtoken:
n = nexttoken(t, &tok);
if (n == 0)
{
return 1;
}
t += n;
switch(*tok) {
case PIPE:
goto newcmd;
break;
case BG:
n = nexttoken(t, &tok);
if (n == 0)
{
c->bakground = 1;
return 1;
}
else
{
fprintf(stderr, "illegal bakgrounding\n");
return -1;
}
break;
case RIN:
if (c->rstdin != NULL)
{
fprintf(stderr, "duplicate redirection of stdin\n");
return -1;
}
if ((n = nexttoken(t, &(c->rstdin))) < 0)
return -1;
if (!isidentifier(c->rstdin))
{
fprintf(stderr, "Illegal filename: \"%s\"\n", c->rstdin);
return -1;
}
t += n;
goto newtoken;
break;
case RUT:
if (c->rstdout != NULL)
{
fprintf(stderr, "duplicate redirection of stdout\n");
return -1;
}
if ((n = nexttoken(t, &(c->rstdout))) < 0)
return -1;
if (!isidentifier(c->rstdout))
{
fprintf(stderr, "Illegal filename: \"%s\"\n", c->rstdout);
return -1;
}
t += n;
goto newtoken;
break;
default:
return -1;
}
goto newcmd;
}
