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

Last change on this file since 343 was 343, checked in by Sanahuja Guillaume, 2 years ago

update imu

File size: 4.2 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 accOffset.x=0;
40 accOffset.y=0;
41 accOffset.z=0;
42 gyrOffset.x=0;
43 gyrOffset.y=0;
44 gyrOffset.z=0;
45 cptOffset=0;
46
47 // station sol
48 mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
49 tab = new TabWidget(mainTab->NewRow(), name);
50 sensorTab = new Tab(tab, "Reglages");
51 setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
52 if(needRotation) {
53 rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation",OneAxisRotation::PostRotation);
54 } else {
55 rotation=NULL;
56 }
57 AddDataToLog(imuData);
58}
59
60Imu::~Imu() {
61 delete mainTab;
62}
63
64const ImuData *Imu::GetDatas(void) const {
65 return imuData;
66}
67
68void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }
69
70OneAxisRotation *Imu::GetOneAxisRotation(void) const {
71 if (rotation == NULL) {
72 Err("not applicable\n");
73 }
74 return rotation;
75}
76
77void Imu::ApplyRotation(Vector3Df& vector) {
78 if (rotation == NULL) {
79 Err("not applicable\n");
80 return;
81 }
82 rotation->ComputeRotation(vector);
83}
84
85void Imu::ApplyRotation(Quaternion& quaternion) {
86 if (rotation == NULL) {
87 Err("not applicable\n");
88 return;
89 }
90 rotation->ComputeRotation(quaternion);
91}
92
93void Imu::RemoveOffsets(Vector3Df& acc,Vector3Df& gyr) {
94 Time time=GetTime();
95
96 if(startCalcOffset==0) {
97 startCalcOffset=time;
98 Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
99 }
100
101 if(!calibrationDone) {
102 if(time<startCalcOffset+(Time)10000000000) {
103 accOffset+=acc;
104 gyrOffset+=gyr;
105 cptOffset++;
106 } else {
107 Printf("%s calibration done\n",ObjectName().c_str());
108 SetIsReady(true);
109 calibrationDone=true;
110 }
111 acc.x=0;
112 acc.y=0;
113 acc.z=0;
114 gyr.x=0;
115 gyr.y=0;
116 gyr.z=0;
117 } else { //calibrationDone
118 acc-=accOffset/cptOffset;
119 gyr-=gyrOffset/cptOffset;
120 }
121
122}
123
124
125GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }
126
127Layout *Imu::GetLayout(void) const { return sensorTab; }
128
129void Imu::LockUserInterface(void) const {
130 sensorTab->setEnabled(false);
131}
132
133void Imu::UnlockUserInterface(void) const {
134 sensorTab->setEnabled(true);
135}
136
137void Imu::UseDefaultPlot(void) {
138
139 plotTab = new Tab(tab, "IMU");
140 axPlot = new DataPlot1D(plotTab->NewRow(), "acc_x", -10, 10);
141 axPlot->AddCurve(imuData->Element(ImuData::RawAx));
142 ayPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_y", -10, 10);
143 ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
144 azPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_z", -10, 10);
145 azPlot->AddCurve(imuData->Element(ImuData::RawAz));
146
147 gxPlot = new DataPlot1D(plotTab->NewRow(), "gyr_x", -500, 500);
148 gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
149 gyPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_y", -500, 500);
150 gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
151 gzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_z", -500, 500);
152 gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));
153
154 mxPlot = new DataPlot1D(plotTab->NewRow(), "mag_x", -500, 500);
155 mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
156 myPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_y", -500, 500);
157 myPlot->AddCurve(imuData->Element(ImuData::RawMy));
158 mzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_z", -500, 500);
159 mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
160}
161
162Tab *Imu::GetPlotTab(void) const { return plotTab; }
163
164} // end namespace sensor
165} // end namespace flair
Note: See TracBrowser for help on using the repository browser.