void Logger::addEWstatic() {
add_datapoint("errCode","-",&errCode);
for(int i = 0; i < 20; i++) {
char name[100];
sprintf(name,"tmpOut[%d]",i);
add_datapoint(name,"-",&(tmpOut[i]));
}
}
void Logger::add_quat(const char *names, Eigen::Quaterniond *q) {
qPoint[nQuat] = q;
char name[100];
sprintf(name, "%s[R]",names);
add_datapoint(name, "r", &(EAbuff[nQuat*3]));
sprintf(name, "%s[P]",names);
add_datapoint(name, "r", &(EAbuff[nQuat*3+1]));
sprintf(name, "%s[Y]",names);
add_datapoint(name, "r", &(EAbuff[nQuat*3+2]));
nQuat++;
}
void DataParser::add_quat(const std::string &names, const Eigen::Quaterniond *q)
{
char name[100];
sprintf(name,"%s[X]",names.c_str());
add_datapoint(name, q->coeffs().data());
sprintf(name,"%s[Y]",names.c_str());
add_datapoint(name, q->coeffs().data()+1);
sprintf(name,"%s[Z]",names.c_str());
add_datapoint(name, q->coeffs().data()+2);
sprintf(name,"%s[W]",names.c_str());
add_datapoint(name, q->coeffs().data()+3);
}
void Logger::init_(double timestep)
{
_ctr = 0;
_myIdx = 0;
_nPoints = 0;
_nChannels = 0;
_data = new float[LOGGER_MAX_N_POINTS*LOGGER_MAX_CHANNELS];
_inited = true; //has to be after memory is allocated
//_frequency = 333;
_frequency = 1./timestep;
add_datapoint("ctr","-",&_ctr);
}
void Logger::add_quat(const std::string &names, const Eigen::Quaterniond *q) {
if (_nQuat < LOGGER_MAX_QUAT) {
_qPtr[_nQuat] = q;
char name[100];
sprintf(name, "%s[RR]",names.c_str());
add_datapoint(name, "r", &(_EAbuff[_nQuat*3]));
sprintf(name, "%s[PP]",names.c_str());
add_datapoint(name, "r", &(_EAbuff[_nQuat*3+1]));
sprintf(name, "%s[YY]",names.c_str());
add_datapoint(name, "r", &(_EAbuff[_nQuat*3+2]));
sprintf(name, "%s[X]",names.c_str());
add_datapoint(name, "-", q->coeffs().data());
sprintf(name, "%s[Y]",names.c_str());
add_datapoint(name, "-", q->coeffs().data()+1);
sprintf(name, "%s[Z]",names.c_str());
add_datapoint(name, "-", q->coeffs().data()+2);
sprintf(name, "%s[W]",names.c_str());
add_datapoint(name, "-", q->coeffs().data()+3);
_nQuat++;
}
}
