source: flair-src/trunk/lib/FlairSensorActuator/src/SimuImu.cpp@ 10

Last change on this file since 10 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 2.9 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/02/07
6// filename: SimuImu.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Class for a simulation imu
14//
15//
16/*********************************************************************/
17
18#include "SimuImu.h"
19#include <FrameworkManager.h>
20#include <ImuData.h>
21#include <SpinBox.h>
22#include <GroupBox.h>
23#include <cvmatrix.h>
24#include <SharedMem.h>
25#include <AhrsData.h>
26#include <sstream>
27
28using std::string;
29using std::ostringstream;
30using namespace flair::core;
31using namespace flair::gui;
32
33namespace flair { namespace sensor {
34
35SimuImu::SimuImu(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) :Imu(parent,name),Thread(parent,name,priority) {
36 data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,200);
37 ahrsData=new AhrsData((Imu*)this);
38
39 ostringstream dev_name;
40 dev_name << "simu_imu_" << dev_id;
41 shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
42}
43
44SimuImu::SimuImu(const IODevice* parent,string name,uint32_t dev_id) :Imu(parent,name),Thread(parent,name,0) {
45 data_rate=NULL;
46
47 ostringstream dev_name;
48 dev_name << "simu_imu_" << dev_id;
49 shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
50}
51
52SimuImu::~SimuImu() {
53 SafeStop();
54 Join();
55}
56
57void SimuImu::UpdateFrom(const io_data *data) {
58 if(data!=NULL) {
59 cvmatrix *input=(cvmatrix*)data;
60 imu_states_t state;
61
62 input->GetMutex();
63 state.q0=input->ValueNoMutex(0,0);
64 state.q1=input->ValueNoMutex(1,0);
65 state.q2=input->ValueNoMutex(2,0);
66 state.q3=input->ValueNoMutex(3,0);
67 state.wx=input->ValueNoMutex(7,0);
68 state.wy=input->ValueNoMutex(8,0);
69 state.wz=input->ValueNoMutex(9,0);
70 input->ReleaseMutex();
71
72 shmem->Write((char*)&state,sizeof(imu_states_t));
73 }
74}
75
76void SimuImu::Run(void) {
77 imu_states_t state;
78 ImuData* imuData;
79 GetDatas(&imuData);
80
81 if(data_rate==NULL) {
82 Thread::Err("not applicable for simulation part.\n");
83 return;
84 }
85
86 SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
87
88 while(!ToBeStopped()) {
89 WaitPeriod();
90
91 if(data_rate->ValueChanged()==true) {
92 SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
93 }
94
95 shmem->Read((char*)&state,sizeof(imu_states_t));
96 ahrsData->SetQuaternionAndAngularRates(Quaternion(state.q0,state.q1,state.q2,state.q3)
97 ,Vector3D(state.wx,state.wy,state.wz));
98
99 imuData->SetDataTime(GetTime());
100 ahrsData->SetDataTime(GetTime());
101
102 UpdateImu();
103 ProcessUpdate(ahrsData);
104 }
105}
106
107} // end namespace sensor
108} // end namespace flair
Note: See TracBrowser for help on using the repository browser.