source: flair-src/trunk/lib/FlairSensorActuator/src/Imu.cpp

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2014/01/16
//  filename:   Imu.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Virtual class for Imu
//
//
/*********************************************************************/

#include "Imu.h"
#include <FrameworkManager.h>
#include <Tab.h>
#include <TabWidget.h>
#include <GroupBox.h>
#include <GridLayout.h>
#include <DataPlot1D.h>
#include <ImuData.h>
#include <OneAxisRotation.h>

using std::string;
using namespace flair::core;
using namespace flair::gui;

namespace flair {
namespace sensor {

Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) {
  imuData = new ImuData(this);
  startCalcOffset=0;
  calibrationDone=false;

  // station sol
  mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
  tab = new TabWidget(mainTab->NewRow(), name);
  sensorTab = new Tab(tab, "Reglages");
  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
  if(needRotation) {
    rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation",OneAxisRotation::PostRotation);
  } else {
    rotation=NULL;
  }
  AddDataToLog(imuData);
}

Imu::~Imu() {
  delete mainTab;
}

const ImuData *Imu::GetDatas(void) const {
  return imuData;
}

void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }

OneAxisRotation *Imu::GetOneAxisRotation(void) const {
  if (rotation == NULL) {
    Err("not applicable\n");
  }
  return rotation;
}

void Imu::ApplyRotation(Vector3Df& vector) {
  if (rotation == NULL) {
    Err("not applicable\n");
    return;
  }
  rotation->ComputeRotation(vector);
}

void Imu::ApplyRotation(Quaternion& quaternion) {
  if (rotation == NULL) {
    Err("not applicable\n");
    return;
  }
  rotation->ComputeRotation(quaternion);
}

void Imu::RemoveOffsets(Vector3Df& acc,Vector3Df& gyr) {
  Time time=GetTime();  
  
  if(startCalcOffset==0) {
    startCalcOffset=time;
    accOffset.x=0;
    accOffset.y=0;
    accOffset.z=0;
    gyrOffset.x=0;
    gyrOffset.y=0;
    gyrOffset.z=0;
    cptOffset=0;
    accMin=acc;
    accMax=acc;
    Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
  } 
  
  if(!calibrationDone) {
    if(time<startCalcOffset+(Time)10000000000) {//calibrating
      accOffset+=acc;
      gyrOffset+=gyr;
      cptOffset++;
      if(acc.GetNorm()<accMin.GetNorm()) accMin=acc;
      if(acc.GetNorm()>accMax.GetNorm()) accMax=acc;
    } else {//check if imu moved or not
      if((accMax-accMin).GetNorm()>0.5) {//put a parameter
         Printf("%s imu was moved, calibrating again!\n",ObjectName().c_str());
         startCalcOffset=0;
      } else {
        Printf("%s calibration done\n",ObjectName().c_str());
        SetIsReady(true);
        accOffset=accOffset/cptOffset;
        gyrOffset=gyrOffset/cptOffset;
        calibrationDone=true;
      }
    } 
    acc.x=0;
    acc.y=0;
    acc.z=0;
    gyr.x=0;
    gyr.y=0;
    gyr.z=0;
  } else { //calibrationDone
     acc-=accOffset;
     gyr-=gyrOffset;
  }
  
}


GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }

Layout *Imu::GetLayout(void) const { return sensorTab; }

void Imu::LockUserInterface(void) const {
  sensorTab->setEnabled(false);
}

void Imu::UnlockUserInterface(void) const {
  sensorTab->setEnabled(true);
}

void Imu::UseDefaultPlot(void) {

  plotTab = new Tab(tab, "IMU");
  axPlot = new DataPlot1D(plotTab->NewRow(), "acc_x", -10, 10);
  axPlot->AddCurve(imuData->Element(ImuData::RawAx));
  ayPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_y", -10, 10);
  ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
  azPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_z", -10, 10);
  azPlot->AddCurve(imuData->Element(ImuData::RawAz));

  gxPlot = new DataPlot1D(plotTab->NewRow(), "gyr_x", -500, 500);
  gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
  gyPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_y", -500, 500);
  gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
  gzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_z", -500, 500);
  gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));

  mxPlot = new DataPlot1D(plotTab->NewRow(), "mag_x", -500, 500);
  mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
  myPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_y", -500, 500);
  myPlot->AddCurve(imuData->Element(ImuData::RawMy));
  mzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_z", -500, 500);
  mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
}

Tab *Imu::GetPlotTab(void) const { return plotTab; }

} // end namespace sensor
} // end namespace flair