source: flair-src/trunk/lib/FlairSimulator/src/SimuImu.cpp@ 310

Last change on this file since 310 was 224, checked in by Sanahuja Guillaume, 7 years ago

maj for armv5te

File size: 1.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 <Matrix.h>
20#include <SharedMem.h>
21#include <sstream>
22
23using std::string;
24using std::ostringstream;
25using namespace flair::core;
26
27namespace flair {
28namespace sensor {
29
30SimuImu::SimuImu(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)
31 : IODevice(parent,name) {
32
33 shmem = new SharedMem(this, ShMemName(modelId, deviceId),
34 sizeof(imu_states_t));
35 SetIsReady(true);
36}
37
38SimuImu::~SimuImu() {
39}
40
41string SimuImu::ShMemName(uint32_t modelId,uint32_t deviceId) {
42 ostringstream dev_name;
43 dev_name << "simu" << modelId << "_imu_" << deviceId;
44 return dev_name.str().c_str();
45}
46
47void SimuImu::UpdateFrom(const io_data *data) {
48 if (data != NULL) {
49 Matrix *input = (Matrix *)data;
50 imu_states_t state;
51
52 input->GetMutex();
53 state.q0 = input->ValueNoMutex(0, 0);
54 state.q1 = input->ValueNoMutex(1, 0);
55 state.q2 = input->ValueNoMutex(2, 0);
56 state.q3 = input->ValueNoMutex(3, 0);
57 state.wx = input->ValueNoMutex(7, 0);
58 state.wy = input->ValueNoMutex(8, 0);
59 state.wz = input->ValueNoMutex(9, 0);
60 state.ax = input->ValueNoMutex(13, 0);
61 state.ay = input->ValueNoMutex(14, 0);
62 state.az = input->ValueNoMutex(15, 0);
63 state.mx = input->ValueNoMutex(16, 0);
64 state.my = input->ValueNoMutex(17, 0);
65 state.mz = input->ValueNoMutex(18, 0);
66 input->ReleaseMutex();
67
68 shmem->Write((char *)&state, sizeof(imu_states_t));
69 }
70}
71
72
73} // end namespace sensor
74} // end namespace flair
Note: See TracBrowser for help on using the repository browser.