JAHRSCalibrationToolkit.hh

Go to the documentation of this file.

#ifndef __JDB_JAHRSCALIBRATIONTOOLKIT__
#define __JDB_JAHRSCALIBRATIONTOOLKIT__
 
#include "JDB/JAHRSCalibration.hh"
 
#include "JTools/JRange.hh"
#include "JLang/JLangToolkit.hh"
#include "JLang/JManip.hh"
 
/**
 * \author mdejong
 */
namespace JDATABASE {}
namespace JPP { using namespace JDATABASE; }
 
namespace JDATABASE {
 
 
  /**
   * Read AHRS calibration from input stream.
   *
   * \param  in            input stream
   * \param  calibration   AHRS calibration
   * \return               input stream
   */
  inline std::istream& operator>>(std::istream& in, JAHRSCalibration& calibration)
  {
    using namespace std;
    using namespace JPP;
 
    in >> calibration.ACC_OFFSET_X
       >> calibration.ACC_OFFSET_Y
       >> calibration.ACC_OFFSET_Z;
 
    in >> calibration.ACC_ROT_XX >> calibration.ACC_ROT_XY >> calibration.ACC_ROT_XZ;
    in >> calibration.ACC_ROT_YX >> calibration.ACC_ROT_YY >> calibration.ACC_ROT_YZ;
    in >> calibration.ACC_ROT_ZX >> calibration.ACC_ROT_ZY >> calibration.ACC_ROT_ZZ;
 
    in >> calibration.MAG_XMIN
       >> calibration.MAG_YMIN
       >> calibration.MAG_ZMIN;
    
    in >> calibration.MAG_XMAX
       >> calibration.MAG_YMAX
       >> calibration.MAG_ZMAX;
    
    in >> calibration.MAG_ROT_XX >> calibration.MAG_ROT_XY >> calibration.MAG_ROT_XZ;
    in >> calibration.MAG_ROT_YX >> calibration.MAG_ROT_YY >> calibration.MAG_ROT_YZ;
    in >> calibration.MAG_ROT_ZX >> calibration.MAG_ROT_ZY >> calibration.MAG_ROT_ZZ;
 
    return in;
  }
 
 
  /**
   * Write AHRS calibration to output stream.
   *
   * \param  out           output stream
   * \param  calibration   AHRS calibration
   * \return               output stream
   */
  inline std::ostream& operator<<(std::ostream& out, const JAHRSCalibration& calibration)
  {
    using namespace std;
    using namespace JPP;
 
    out << FIXED(9,6) << calibration.ACC_OFFSET_X << ' '
        << FIXED(9,6) << calibration.ACC_OFFSET_Y << ' '
        << FIXED(9,6) << calibration.ACC_OFFSET_Z << ' ';
 
    out << FIXED(9,6) << calibration.ACC_ROT_XX << ' ' << FIXED(9,6) << calibration.ACC_ROT_XY << ' ' << FIXED(9,6) << calibration.ACC_ROT_XZ << ' ';
    out << FIXED(9,6) << calibration.ACC_ROT_YX << ' ' << FIXED(9,6) << calibration.ACC_ROT_YY << ' ' << FIXED(9,6) << calibration.ACC_ROT_YZ << ' ';
    out << FIXED(9,6) << calibration.ACC_ROT_ZX << ' ' << FIXED(9,6) << calibration.ACC_ROT_ZY << ' ' << FIXED(9,6) << calibration.ACC_ROT_ZZ << ' ';
 
    out << FIXED(9,6) << calibration.MAG_XMIN << ' '
        << FIXED(9,6) << calibration.MAG_YMIN << ' '
        << FIXED(9,6) << calibration.MAG_ZMIN << ' ';
    
    out << FIXED(9,6) << calibration.MAG_XMAX << ' '
        << FIXED(9,6) << calibration.MAG_YMAX << ' '
        << FIXED(9,6) << calibration.MAG_ZMAX << ' ';
    
    out << FIXED(9,6) << calibration.MAG_ROT_XX << ' ' << FIXED(9,6) << calibration.MAG_ROT_XY << ' ' << FIXED(9,6) << calibration.MAG_ROT_XZ << ' ';
    out << FIXED(9,6) << calibration.MAG_ROT_YX << ' ' << FIXED(9,6) << calibration.MAG_ROT_YY << ' ' << FIXED(9,6) << calibration.MAG_ROT_YZ << ' ';
    out << FIXED(9,6) << calibration.MAG_ROT_ZX << ' ' << FIXED(9,6) << calibration.MAG_ROT_ZY << ' ' << FIXED(9,6) << calibration.MAG_ROT_ZZ << ' ';
 
    return out;
  }
 
 
  /**
   * Get numerical value of AHRS calibration version.
   *
   * \param  version       version
   * \return               version
   */
  inline int getVersion(const std::string& version)
  {
    using namespace std;
    using namespace JPP;
 
    size_t pos = version.find_last_of("vV");
 
    if (pos != string::npos) 
      return to_value<int>(version.substr(pos+1));
    else
      return -1;
  }
 
  
  /**
   * Auxiliary data structure to check validity of AHRS calibration data.
   */
  struct JAHRSCalibrationValidity {
 
    typedef JTOOLS::JRange<double> range_type;
 
    /**
     * Default constructor.
     *
     * This constructor sets default ranges of acceptance of AHRS calibration values.
     */
    JAHRSCalibrationValidity() 
    {
      ACC_OFFSET_X  = range_type(-1.0, +1.0);
      ACC_OFFSET_Y  = range_type(-1.0, +1.0);
      ACC_OFFSET_Z  = range_type(-1.0, +1.0);
 
      ACC_ROT[0][0] = range_type(-1.8, +1.8);  ACC_ROT[0][1] = range_type(-1.0, +1.0);  ACC_ROT[0][2] = range_type(-1.0, +1.0);
      ACC_ROT[1][0] = range_type(-1.0, +1.0);  ACC_ROT[1][1] = range_type(+0.6, +1.8);  ACC_ROT[1][2] = range_type(-1.0, +1.0);
      ACC_ROT[2][0] = range_type(-1.0, +1.0);  ACC_ROT[2][1] = range_type(-1.0, +1.0);  ACC_ROT[2][2] = range_type(+0.6, +1.8);
    
      MAG_OFFSET_X  = range_type(-1.0, +1.0);
      MAG_OFFSET_Y  = range_type(-1.0, +1.0);
      MAG_OFFSET_Z  = range_type(-1.0, +1.0);
 
      MAG_ROT[0][0] = range_type(-1.8, +1.8);  MAG_ROT[0][1] = range_type(-1.0, +1.0);  MAG_ROT[0][2] = range_type(-1.0, +1.0);
      MAG_ROT[1][0] = range_type(-1.0, +1.0);  MAG_ROT[1][1] = range_type(+0.6, +1.8);  MAG_ROT[1][2] = range_type(-1.0, +1.0);
      MAG_ROT[2][0] = range_type(-1.0, +1.0);  MAG_ROT[2][1] = range_type(-1.0, +1.0);  MAG_ROT[2][2] = range_type(+0.6, +1.8);    
    }
 
 
    /**
     * Check validity of AHRS calibration data.
     *
     * \param  calibration   AHRS calibration
     * \return               true if valid; else false
     */
    bool operator()(const JAHRSCalibration& calibration) const
    {
      return (ACC_OFFSET_X(calibration.ACC_OFFSET_X)   &&
              ACC_OFFSET_Y(calibration.ACC_OFFSET_Y)   &&
              ACC_OFFSET_Z(calibration.ACC_OFFSET_Z)   &&
            
              ACC_ROT[0][0](calibration.ACC_ROT_XX)  &&  ACC_ROT[0][1](calibration.ACC_ROT_XY)  &&  ACC_ROT[0][2](calibration.ACC_ROT_XZ)  &&
              ACC_ROT[1][0](calibration.ACC_ROT_YX)  &&  ACC_ROT[1][1](calibration.ACC_ROT_YY)  &&  ACC_ROT[1][2](calibration.ACC_ROT_YZ)  &&
              ACC_ROT[2][0](calibration.ACC_ROT_ZX)  &&  ACC_ROT[2][1](calibration.ACC_ROT_ZY)  &&  ACC_ROT[2][2](calibration.ACC_ROT_ZZ)  &&
 
              MAG_OFFSET_X(0.5 * (calibration.MAG_XMIN + calibration.MAG_XMAX))  &&
              MAG_OFFSET_Y(0.5 * (calibration.MAG_YMIN + calibration.MAG_YMAX))  &&
              MAG_OFFSET_Z(0.5 * (calibration.MAG_ZMIN + calibration.MAG_ZMAX))  &&
              
              MAG_ROT[0][0](calibration.MAG_ROT_XX)  &&  MAG_ROT[0][1](calibration.MAG_ROT_XY)  &&  MAG_ROT[0][2](calibration.MAG_ROT_XZ)  &&
              MAG_ROT[1][0](calibration.MAG_ROT_YX)  &&  MAG_ROT[1][1](calibration.MAG_ROT_YY)  &&  MAG_ROT[1][2](calibration.MAG_ROT_YZ)  &&
              MAG_ROT[2][0](calibration.MAG_ROT_ZX)  &&  MAG_ROT[2][1](calibration.MAG_ROT_ZY)  &&  MAG_ROT[2][2](calibration.MAG_ROT_ZZ));
    }
 
 
    range_type ACC_OFFSET_X;
    range_type ACC_OFFSET_Y;
    range_type ACC_OFFSET_Z;
 
    range_type ACC_ROT[3][3];
    
    range_type MAG_OFFSET_X;
    range_type MAG_OFFSET_Y;
    range_type MAG_OFFSET_Z;
 
    range_type MAG_ROT[3][3];
  };
 
 
  /**
   * Auxiliary data structure for sorting of AHRS calibrations.
   */
  struct JAHRSCalibrationComparator {
    /**
     * Comparison of operation identifiers.
     *
     * \param  first         first  operation identifiers
     * \param  second        second operation identifiers
     * \return               true if first calibration better than second; else false
     */
    inline bool operator()(const std::string& first,
                           const std::string& second) const
    {
      using namespace std;
      using namespace JPP;
 
      if (!first.empty() && !second.empty()) {
 
        if (first[0] == second[0])
          return to_value<int>(first.substr(1)) > to_value<int>(second.substr(1));
        else
          return first[0] > second[0];
 
      } else {
 
        return first > second;
      }
    }
 
 
    /**
     * Comparison of AHRS calibrations.
     *
     * \param  first         first  AHRS calibration
     * \param  second        second AHRS calibration
     * \return               true if first AHRS calibration better than second; else false
     */
    inline bool operator()(const JAHRSCalibration& first,
                           const JAHRSCalibration& second) const
    {
      if (first.SERIALNUMBER == second.SERIALNUMBER) {
 
        if (first.TESTEND == second.TESTEND) {
 
          if (getVersion(first.TESTNAME) == getVersion(second.TESTNAME))
            return (*this)(first.TESTOPID, second.TESTOPID);
          else
            return getVersion(first.TESTNAME) > getVersion(second.TESTNAME);
 
        } else {
          
          return first.TESTEND > second.TESTEND;
        }
        
      } else {
 
        return first.SERIALNUMBER > second.SERIALNUMBER;
      }
    }
  };
}
 
#endif