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

Last change on this file since 4 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.