source: flair-src/trunk/lib/FlairIpc/src/IpcImu.cpp @ 397

Last change on this file since 397 was 397, checked in by Sanahuja Guillaume, 7 months ago

add ipc lib

  • Property svn:eol-style set to native
File size: 3.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:    2021/03/03
6//  filename:   IpcImu.cpp
7//
8//  author:     Sébastien Ambroziak
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class for an ipc imu
14//
15//
16/*********************************************************************/
17
18#include "IpcImu.h"
19#include "ipc_receive.h"
20#include <FrameworkManager.h>
21#include <ImuData.h>
22#include <SpinBox.h>
23#include <GroupBox.h>
24#include <Matrix.h>
25#include <AhrsData.h>
26
27using namespace flair::core;
28using namespace flair::gui;
29
30namespace flair {
31namespace sensor {
32
33IpcImu::IpcImu(std::string name, uint8_t priority, const char* ipc_name, int ipc_channel, bool AhrsOnly)
34    : Imu(name,false), Thread(getFrameworkManager(), name, priority) {
35  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
36  ahrsData = new AhrsData((Imu *)this);
37  _AhrsOnly = AhrsOnly;
38  if (AhrsOnly){
39    //AhrsReceiver = new IpcReceiver<ipc::type::ahrs>(ipc_name,ipc_channel);
40    orientation_quat = new IpcReceiver<ipc::type::vector4D_32>("ipc_quat",65);
41    angular_vel = new IpcReceiver<ipc::type::vector3D_32>("ipc_angl",65);
42  }
43  else{
44    ImuReceiver = new IpcReceiver<ipc::type::imu>(ipc_name,ipc_channel);
45  }
46 
47  SetIsReady(true);
48}
49
50IpcImu::~IpcImu() {
51  SafeStop();
52  Join();
53}
54
55
56void IpcImu::Run(void) {
57  imu_states_t state;
58  ImuData *imuData;
59  ipc::type::imu imu_msg;
60  ipc::type::ahrs ahrs_msg;
61  ipc::type::vector4D_32 quat_msg;
62  ipc::type::vector3D_32 angl_msg;
63  GetDatas(&imuData);
64
65  Quaternion quaternion;
66  Vector3Df angRate;
67  Vector3Df rawAcc;
68  Vector3Df rawMag;
69  Vector3Df rawGyr;
70  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
71
72  while (!ToBeStopped()) {
73
74    angRate.x = 0;
75    angRate.y = 0;
76    angRate.z = 0;
77
78    rawAcc.x = 0;
79    rawAcc.y = 0;
80    rawAcc.z = 0;
81
82    rawMag.x = 0;
83    rawMag.y = 0;
84    rawMag.z = 0;
85
86    rawGyr.x = 0;
87    rawGyr.y = 0;
88    rawGyr.z = 0;
89
90    if (_AhrsOnly){
91      //AhrsReceiver->receive();
92      //ahrs_msg = AhrsReceiver->getValue();
93     
94      orientation_quat->receive();
95      angular_vel->receive();
96      quat_msg = orientation_quat->getValue();
97      angl_msg = angular_vel->getValue();
98
99      quaternion.q0 = quat_msg.x;
100      quaternion.q1 = quat_msg.y;
101      quaternion.q2 = quat_msg.z;
102      quaternion.q3 = quat_msg.w;
103
104      angRate.x = angl_msg.x;
105      angRate.y = angl_msg.y;
106      angRate.z = angl_msg.z;
107
108      imuData->SetDataTime(AhrsReceiver->getTimestamp().toNanosec());
109      ahrsData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
110    }
111    else {
112      ImuReceiver->receive();
113
114      imu_msg = ImuReceiver->getValue();
115
116      quaternion.q0 = imu_msg.orientation.w;
117      quaternion.q1 = imu_msg.orientation.x;
118      quaternion.q2 = imu_msg.orientation.y;
119      quaternion.q3 = imu_msg.orientation.z;
120
121      rawAcc.x = imu_msg.linear_acceleration.x;
122      rawAcc.y = imu_msg.linear_acceleration.y;
123      rawAcc.z = imu_msg.linear_acceleration.z;
124
125      /*rawGyr.x = imu_msg.angular_velocity.x;
126      rawGyr.y = imu_msg.angular_velocity.y;
127      rawGyr.z = imu_msg.angular_velocity.z;*/
128
129      angRate.x = imu_msg.angular_velocity.x;
130      angRate.y = imu_msg.angular_velocity.y;
131      angRate.z = imu_msg.angular_velocity.z;
132
133      imuData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
134      ahrsData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
135    }
136
137   
138   
139    //we do not need rotation in simulation
140    /*
141    ApplyRotation(angRate);
142    ApplyRotation(quaternion);
143    ApplyRotation(rawAcc);
144    ApplyRotation(rawMag);
145    ApplyRotation(rawGyr);*/
146    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
147    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
148   
149    //ahrsData->SetDataTime(GetTime());
150    ProcessUpdate(ahrsData);
151  }
152}
153
154} // end namespace sensor
155} // end namespace flair
Note: See TracBrowser for help on using the repository browser.