Skip to content

GitLab

Commit de9d7024 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix stdout and indentation.

parent 7afaa6eb
Hide whitespace changes
Inline Side-by-side
include/sot/core/debug.hh
View file @ de9d7024
......@@ -24,180 +24,183 @@
# endif //! VP_TEMPLATE_DEBUG_MODE
# define SOT_COMMON_TRACES \
do { \
va_list arg; \
va_start(arg,format); \
vsnprintf( charbuffer,SIZE,format,arg ); \
va_end(arg); \
outputbuffer << tmpbuffer.str() << charbuffer <<std::endl; \
} while(0)
namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT DebugTrace
do { \
va_list arg; \
va_start(arg,format); \
vsnprintf( charbuffer,SIZE,format,arg ); \
va_end(arg); \
outputbuffer << tmpbuffer.str() << charbuffer <<std::endl; \
} while(0)
namespace dynamicgraph
{
namespace sot
{
public:
static const int SIZE = 512;
std::stringstream tmpbuffer;
std::ostream& outputbuffer;
char charbuffer[SIZE+1];
int traceLevel;
int traceLevelTemplate;
DebugTrace( std::ostream& os ): outputbuffer(os) {}
inline void trace(const int level,const char* format, ...)
{
if (level<=traceLevel)
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void trace(const char* format,...)
{
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void trace(const int level=-1)
{
if (level<=traceLevel)
outputbuffer << tmpbuffer.str(); tmpbuffer.str("");
}
inline void traceTemplate(const int level, const char* format, ...)
{
if (level<=traceLevelTemplate)
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void traceTemplate(const char* format, ...)
{
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline DebugTrace& pre(const std::ostream&)
{
return *this;
}
inline DebugTrace& pre(const std::ostream&, int level)
{
traceLevel = level;
return *this;
}
static const char* DEBUG_FILENAME_DEFAULT;
static void openFile(const char * filename = DEBUG_FILENAME_DEFAULT);
static void closeFile(const char * filename = DEBUG_FILENAME_DEFAULT);
};
SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
} // namespace sot
class SOT_CORE_EXPORT DebugTrace
{
public:
static const int SIZE = 512;
std::stringstream tmpbuffer;
std::ostream& outputbuffer;
char charbuffer[SIZE+1];
int traceLevel;
int traceLevelTemplate;
DebugTrace( std::ostream& os ): outputbuffer(os) {}
inline void trace(const int level,const char* format, ...)
{
if (level<=traceLevel)
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void trace(const char* format,...)
{
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void trace(const int level=-1)
{
if (level<=traceLevel)
outputbuffer << tmpbuffer.str();
tmpbuffer.str("");
}
inline void traceTemplate(const int level, const char* format, ...)
{
if (level<=traceLevelTemplate)
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline void traceTemplate(const char* format, ...)
{
SOT_COMMON_TRACES;
tmpbuffer.str("");
}
inline DebugTrace& pre(const std::ostream&)
{
return *this;
}
inline DebugTrace& pre(const std::ostream&, int level)
{
traceLevel = level;
return *this;
}
static const char* DEBUG_FILENAME_DEFAULT;
static void openFile(const char * filename = DEBUG_FILENAME_DEFAULT);
static void closeFile(const char * filename = DEBUG_FILENAME_DEFAULT);
};
SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
} // namespace sot
} // namespace dynamicgraph
# ifdef VP_DEBUG
# define sotPREDEBUG \
__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
# define sotPREERROR \
"\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
"\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
# define sotDEBUG(level) \
if ((level>VP_DEBUG_MODE) || (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
if ((level>VP_DEBUG_MODE) || (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotDEBUGMUTE(level) \
if( (level>VP_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
if( (level>VP_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) ) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
# define sotERROR \
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else dynamicgraph::sot::sotERRORFLOW.outputbuffer << sotPREERROR
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else dynamicgraph::sot::sotERRORFLOW.outputbuffer << sotPREERROR
# define sotDEBUGF \
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.pre \
(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.pre \
(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
# define sotERRORF \
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
// TEMPLATE
# define sotTDEBUG(level) \
if((level>VP_TEMPLATE_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
if((level>VP_TEMPLATE_DEBUG_MODE)||(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotTDEBUGF \
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.pre \
(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
; \
else \
dynamicgraph::sot::sotDEBUGFLOW.pre \
(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
namespace dynamicgraph {
namespace sot {
inline bool sotDEBUG_ENABLE(const int & level)
{
return level <= VP_DEBUG_MODE;
}
inline bool sotTDEBUG_ENABLE(const int & level)
{
return level<=VP_TEMPLATE_DEBUG_MODE;
}
} // namespace sot
namespace sot {
inline bool sotDEBUG_ENABLE(const int & level)
{
return level <= VP_DEBUG_MODE;
}
inline bool sotTDEBUG_ENABLE(const int & level)
{
return level<=VP_TEMPLATE_DEBUG_MODE;
}
} // namespace sot
} // namespace dynamicgraph
/* -------------------------------------------------------------------------- */
# else // VP_DEBUG
# define sotPREERROR \
"\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
"\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
# define sotDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
# define sotDEBUGMUTE(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
# define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
namespace dynamicgraph {
namespace sot {
inline void sotDEBUGF(const int,const char*,...) {}
inline void sotDEBUGF(const char*,...) {}
inline void sotERRORF(const int, const char*,...) {}
inline void sotERRORF(const char*,...) {}
inline std::ostream& __null_stream () {
// This function should never be called. With -O3,
// it should not appear in the generated binary.
static std::ostream os (NULL); return os;
}
} // namespace sot
namespace sot {
inline void sotDEBUGF(const int,const char*,...) {}
inline void sotDEBUGF(const char*,...) {}
inline void sotERRORF(const int, const char*,...) {}
inline void sotERRORF(const char*,...) {}
inline std::ostream& __null_stream () {
// This function should never be called. With -O3,
// it should not appear in the generated binary.
static std::ostream os (NULL); return os;
}
} // namespace sot
} // namespace dynamicgraph
// TEMPLATE
# define sotTDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
namespace dynamicgraph {
namespace sot {
inline void sotTDEBUGF(const int, const char*,...) {}
inline void sotTDEBUGF(const char*,...) {}
} // namespace sot
namespace sot {
inline void sotTDEBUGF(const int, const char*,...) {}
inline void sotTDEBUGF(const char*,...) {}
} // namespace sot
} // namespace dynamicgraph
# define sotDEBUG_ENABLE(level) false
......@@ -216,6 +219,6 @@ namespace dynamicgraph {
#endif //! #ifdef SOT_CORE_DEBUG_HH
// Local variables:
// c-basic-offset: 4
// End:
// Local variables:
// c-basic-offset: 2
// End:
include/sot/core/madgwickahrs.hh
View file @ de9d7024
//=====================================================================================================
//========================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
......@@ -9,7 +9,7 @@
// 11/05/2017 T Flayols Make it a dynamic graph entity
// 26/03/2019 G Buondonno Converted to double
//
//=====================================================================================================
//========================================================================
/*
* Copyright 2017, Thomas Flayols, LAAS-CNRS
......@@ -58,10 +58,9 @@
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------- */
/* --------------------------------------------------------------- */
/** \addtogroup Filters
\section subsec_madgwickahrs MadgwickAHRS filter
\class MadgwickARHS
This class implements the MadgwickAHRS filter as described
in http://x-io.co.uk/res/doc/madgwick_internal_report.pdf
This method uses a gradient descent approach to compute the orientation
......@@ -71,7 +70,8 @@ namespace dynamicgraph {
<ul>
<li>m_accelerometerSIN: \f$[a_x, a_y, a_z]^T\f$ in \f$m.s^{-2}\f$</li>
<li>m_gyroscopeSIN: \f$[g_x, g_y, g_z]^T\f$ in \f$rad.s^{-1}\f$</li>
<li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T</li> estimated rotation as a quaternion</li>
<li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T</li> estimated rotation
as a quaternion</li>
</ul>
The internal parameters are:
......
src/tools/device.cpp
View file @ de9d7024
......@@ -13,6 +13,7 @@
/* SOT */
#define ENABLE_RT_LOG
#include <iostream>
#include "sot/core/device.hh"
#include <sot/core/debug.hh>
using namespace std;
......@@ -611,6 +612,6 @@ void Device::display ( std::ostream& os ) const
void Device::cmdDisplay ( )
{
std::cout << name <<": "<<state_<<endl
<< "sanityCheck: " << sanityCheck_<< endl
<< "controlInputType:" << controlInputType_ << endl;
<< "sanityCheck: " << sanityCheck_<< endl
<< "controlInputType:" << controlInputType_ << endl;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment