void KukaRMControllerRTNET::estimateAMAF(Eigen::VectorXd& F)
{
Eigen::VectorXd er(LWRDOF);
Eigen::VectorXd sc(LWRDOF);
Eigen::VectorXd precL(LWRDOF);
double fast = 2/(m_fastLen+1);
double slow = 2/(m_slowLen+1);
for(unsigned int i=0;i<LWRDOF;i++)
{
m_bufSum(i) = m_bufSum(i)-m_FTab(i,m_Fidx);
/* Memorisation de la nbAxe-m_lg valeur */
if(m_FBoucle){
precL(i) = m_buf(i,m_Fidx);
}
else{
precL(i) = m_buf(i,0);
}
/* Mis a jour de la i-ieme valeur */
m_buf(i,m_Fidx) = ((F-m_prec).array().abs())(i);
/* Mise a jour de la somme */
m_bufSum(i) = m_bufSum(i)+m_buf(i,m_Fidx);
/* Calcul de ER */
if(m_bufSum(i)==0){
er(i) = 0;
}
else{
er(i) = ((F-precL).array().abs())(i)/m_bufSum(i);
}
/* Calcul de sc */
sc(i) = (er(i)*(fast-slow)+slow)*(er(i)*(fast-slow)+slow);
/* Calcul de la moyenne mobile */
m_AMA(i) = m_AMA(i)*(1.0-sc(i))+sc(i)*F(i);
/* Sortie de la fonction */
F(i) = m_AMA(i);
m_prec(i) = m_buf(i,m_Fidx);
}
/* Mise a jour des parametres pour la prochaine utilisation */
m_Fidx=m_Fidx+1;
if(m_Fidx>m_L)
{
m_Fidx = 1;
m_FBoucle = 1;
}
if(m_fastLen<m_FL){
m_fastLen = m_fastLen+1;
}
if(m_slowLen<m_SL){
m_slowLen = m_slowLen+1;
}
}
/** create a child process and connect a pipe
to child stdout
*/
static void child_exec(int hc)
{
char *null_string = NULL;
struct child_stat *child = mls(CLIST, hc);
if( pipe2( child->fd, O_NONBLOCK ) ) ERR("pipe2");
if( pipe2( child->fd+2, O_NONBLOCK ) ) ERR("pipe2");
pid_t cpid = fork();
if (cpid == -1) ERR("fork");
if (cpid == 0) { /* Child reads from pipe */
dup2(child->fd[1],1); /* make STDOUT==1 same as WRITE-TO==1 end
of pipe-A */
dup2(child->fd[2],0); /* make STDIN==0 sama es READ-FROM==0 end
of pipe-B */
xclose(child->fd+0); /* Close unused read end pipe-A */
xclose(child->fd+3); /* Close unused write end pipe-B */
m_put(child->args,&null_string);
// execlp( child->filename, child->filename, NULL );
execvp( child->filename, m_buf(child->args) );
m_drop(child->args);
perror( "can not exec child process" );
_exit(EXIT_FAILURE); /* never reached */
}
xclose( child->fd+1 ); /* close unused WRITE end of pipe-A */
xclose( child->fd+2 ); /* close unused READ end of pipe-B */
child->stat = CHILD_RUNNING;
child->pid = cpid;
}
static void dummy_cb(Widget w, void *u, void *d)
{
int line = (intptr_t) d;
if( line < 1 ) return;
TRACE(SRVC, "%s", (char*)m_buf(line));
}
FILE *xopen_file( char *fn, char *mode )
{
int buf = s_printf( 0,0, "%s/%s", rc_key_value("files","datadir"), fn );
FILE *fp= fopen( m_buf(buf), mode );
if( !fp ) WARN("cannot access File %s", fn);
m_free(buf);
return fp;
}
FILE *open_cachfile(int write)
{
const char *s;
if ((s = getenv("HOME")) == NULL) {
s = getpwuid(getuid())->pw_dir;
}
if( s==NULL ) s="/tmp";
int fn = s_printf( 0,0, "%s/%s", s, SETTINGS.cache_dir );
FILE *fp = fopen( m_buf(fn), write ? "w" : "r" );
m_free(fn);
return fp;
}
Logger::Logger() : m_buf(), m_severity(LSEVERITY_DEBUG), m_output(&m_buf), m_null(&null_buffer)
#else
Logger::Logger() : m_buf(), m_severity(LSEVERITY_INFO), m_output(&m_buf), m_null(&null_buffer)
#endif
{
}
std::ostream &Logger::log(LogSeverity sev)
{
const char *sevtext;
if(sev < m_severity)
{
return m_null;
}
switch(sev)
{
case LSEVERITY_PACKET:
sevtext = "PACKET";
break;
case LSEVERITY_TRACE:
sevtext = "TRACE";
break;
case LSEVERITY_DEBUG:
sevtext = "DEBUG";
break;
case LSEVERITY_INFO:
sevtext = "INFO";
break;
case LSEVERITY_WARNING:
sevtext = "WARNING";
break;
case LSEVERITY_SECURITY:
sevtext = "SECURITY";
break;
case LSEVERITY_ERROR:
sevtext = "ERROR";
break;
case LSEVERITY_FATAL:
sevtext = "FATAL";
break;
}
time_t rawtime;
time(&rawtime);
char timetext[1024];
strftime(timetext, 1024, "%Y-%m-%d %H:%M:%S", localtime(&rawtime));
m_output << "[" << timetext << "] " << sevtext << ": ";
return m_output;
}
void Logger::set_min_severity(LogSeverity sev)
{
m_severity = sev;
}
LogSeverity Logger::get_min_severity()
{
return m_severity;
}
char* child_data(int hc)
{
struct child_stat *child = mls(CLIST, hc);
return m_buf(child->line);
}