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

Last change on this file since 415 was 397, checked in by Sanahuja Guillaume, 18 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.