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

Last change on this file was 344, checked in by Sanahuja Guillaume, 21 months ago

improve imu calibration by detecting a movement

File size: 4.7 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2014/01/16
6//  filename:   Imu.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Virtual class for Imu
14//
15//
16/*********************************************************************/
17
18#include "Imu.h"
19#include <FrameworkManager.h>
20#include <Tab.h>
21#include <TabWidget.h>
22#include <GroupBox.h>
23#include <GridLayout.h>
24#include <DataPlot1D.h>
25#include <ImuData.h>
26#include <OneAxisRotation.h>
27
28using std::string;
29using namespace flair::core;
30using namespace flair::gui;
31
32namespace flair {
33namespace sensor {
34
35Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) {
36  imuData = new ImuData(this);
37  startCalcOffset=0;
38  calibrationDone=false;
39
40  // station sol
41  mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
42  tab = new TabWidget(mainTab->NewRow(), name);
43  sensorTab = new Tab(tab, "Reglages");
44  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
45  if(needRotation) {
46    rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation",OneAxisRotation::PostRotation);
47  } else {
48    rotation=NULL;
49  }
50  AddDataToLog(imuData);
51}
52
53Imu::~Imu() {
54  delete mainTab;
55}
56
57const ImuData *Imu::GetDatas(void) const {
58  return imuData;
59}
60
61void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }
62
63OneAxisRotation *Imu::GetOneAxisRotation(void) const {
64  if (rotation == NULL) {
65    Err("not applicable\n");
66  }
67  return rotation;
68}
69
70void Imu::ApplyRotation(Vector3Df& vector) {
71  if (rotation == NULL) {
72    Err("not applicable\n");
73    return;
74  }
75  rotation->ComputeRotation(vector);
76}
77
78void Imu::ApplyRotation(Quaternion& quaternion) {
79  if (rotation == NULL) {
80    Err("not applicable\n");
81    return;
82  }
83  rotation->ComputeRotation(quaternion);
84}
85
86void Imu::RemoveOffsets(Vector3Df& acc,Vector3Df& gyr) {
87  Time time=GetTime(); 
88 
89  if(startCalcOffset==0) {
90    startCalcOffset=time;
91    accOffset.x=0;
92    accOffset.y=0;
93    accOffset.z=0;
94    gyrOffset.x=0;
95    gyrOffset.y=0;
96    gyrOffset.z=0;
97    cptOffset=0;
98    accMin=acc;
99    accMax=acc;
100    Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
101  } 
102 
103  if(!calibrationDone) {
104    if(time<startCalcOffset+(Time)10000000000) {//calibrating
105      accOffset+=acc;
106      gyrOffset+=gyr;
107      cptOffset++;
108      if(acc.GetNorm()<accMin.GetNorm()) accMin=acc;
109      if(acc.GetNorm()>accMax.GetNorm()) accMax=acc;
110    } else {//check if imu moved or not
111      if((accMax-accMin).GetNorm()>0.5) {//put a parameter
112         Printf("%s imu was moved, calibrating again!\n",ObjectName().c_str());
113         startCalcOffset=0;
114      } else {
115        Printf("%s calibration done\n",ObjectName().c_str());
116        SetIsReady(true);
117        accOffset=accOffset/cptOffset;
118        gyrOffset=gyrOffset/cptOffset;
119        calibrationDone=true;
120      }
121    } 
122    acc.x=0;
123    acc.y=0;
124    acc.z=0;
125    gyr.x=0;
126    gyr.y=0;
127    gyr.z=0;
128  } else { //calibrationDone
129     acc-=accOffset;
130     gyr-=gyrOffset;
131  }
132 
133}
134
135
136GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }
137
138Layout *Imu::GetLayout(void) const { return sensorTab; }
139
140void Imu::LockUserInterface(void) const {
141  sensorTab->setEnabled(false);
142}
143
144void Imu::UnlockUserInterface(void) const {
145  sensorTab->setEnabled(true);
146}
147
148void Imu::UseDefaultPlot(void) {
149
150  plotTab = new Tab(tab, "IMU");
151  axPlot = new DataPlot1D(plotTab->NewRow(), "acc_x", -10, 10);
152  axPlot->AddCurve(imuData->Element(ImuData::RawAx));
153  ayPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_y", -10, 10);
154  ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
155  azPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_z", -10, 10);
156  azPlot->AddCurve(imuData->Element(ImuData::RawAz));
157
158  gxPlot = new DataPlot1D(plotTab->NewRow(), "gyr_x", -500, 500);
159  gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
160  gyPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_y", -500, 500);
161  gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
162  gzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_z", -500, 500);
163  gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));
164
165  mxPlot = new DataPlot1D(plotTab->NewRow(), "mag_x", -500, 500);
166  mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
167  myPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_y", -500, 500);
168  myPlot->AddCurve(imuData->Element(ImuData::RawMy));
169  mzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_z", -500, 500);
170  mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
171}
172
173Tab *Imu::GetPlotTab(void) const { return plotTab; }
174
175} // end namespace sensor
176} // end namespace flair
Note: See TracBrowser for help on using the repository browser.