close Warning: Can't use blame annotator: 
svn blame failed on trunk/lib/FlairSimulator/src/Plane.cpp: 200029 - Couldn't perform atomic initialization

source: flair-src/trunk/lib/FlairSimulator/src/Plane.cpp@ 457

Last change on this file since 457 was 457, checked in by Sanahuja Guillaume, 2 years ago

simple plane for Armando
File size: 9.7 KB
RevLine 
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/12/22
6// filename: Plane.cpp
7//
8// author: Armando Alatorre Sevilla, Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: classe definissant un avion
14//
15/*********************************************************************/
16
17#include "Plane.h"
18#include <SimuBldc.h>
19#include <TabWidget.h>
20#include <Tab.h>
21#include <DoubleSpinBox.h>
22#include <SpinBox.h>
23#include <GroupBox.h>
24#include <math.h>
25#ifdef GL
26#include <ISceneManager.h>
27#include <IMeshManipulator.h>
28#include "Blade.h"
29#include "MeshSceneNode.h"
30#include "Gui.h"
31#include <Mutex.h>
32#endif
33
34#define K_MOT 0.4f // blade animation
35#define G (float)9.81 // gravity ( N/(m/s²) )
36
37#ifdef GL
38using namespace irr::video;
39using namespace irr::scene;
40using namespace irr::core;
41#endif
42using namespace flair::core;
43using namespace flair::gui;
44using namespace flair::actuator;
45
46namespace flair {
47namespace simulator {
48
49Plane::Plane(std::string name, uint32_t modelId)
50 : Model(name,modelId) {
51 Tab *setup_tab = new Tab(GetTabWidget(), "model");
52 m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
53
54 motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);
55
56 motor = new SimuBldc(this, name, 1, modelId,0);
57
58 SetIsReady(true);
59}
60
61Plane::~Plane() {
62 // les objets irrlicht seront automatiquement detruits (moteurs, helices,
63 // pales) par parenté
64}
65
66#ifdef GL
67
68void Plane::Draw(void) {
69 // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
70 // parameter
71 // note that the frame used is irrlicht one:
72 // left handed, North East Up
73 const IGeometryCreator *geo;
74 geo = getGui()->getSceneManager()->getGeometryCreator();
75
76 // cylinders are aligned with y axis
77 IMesh *motor_mesh = geo->createCylinderMesh(7.5, 5, 16);
78 ITexture *texture = getGui()->getTexture("metal047.jpg");
79 MeshSceneNode *f_motor = new MeshSceneNode(this, motor_mesh, vector3df(150, 0, 20),
80 vector3df(0, 0, -90), texture);
81 f_blade = new Blade(this, vector3df(155, 0,20), vector3df(90, 0, 90));
82
83 IMesh *body_mesh = geo->createCylinderMesh(20, 150, 32,SColor(0, 255, 255, 255));
84 MeshSceneNode *body = new MeshSceneNode(this, body_mesh, vector3df(0, 0, 20),
85 vector3df(0, 0, -90));
86
87 IMesh *wing= geo->createCubeMesh (vector3df(35, 100, 2));
88 IMesh *aileron= geo->createCubeMesh (vector3df(10, 90, 2));
89 //origine de l'aile en son centre
90 MeshSceneNode *l_wing = new MeshSceneNode(this, wing, vector3df(75, -20-50, 20),
91 vector3df(0, 0, 0));
92 MeshSceneNode *r_wing = new MeshSceneNode(this, wing, vector3df(75, 20+50, 20),
93 vector3df(0, 0, 0));
94
95 l_aileron = new MeshSceneNode(this, aileron, vector3df(55, -20-50, 20),
96 vector3df(0, 0, 0));
97
98 r_aileron = new MeshSceneNode(this, aileron, vector3df(55, 20+50, 20),
99 vector3df(0, 0, 0));
100
101 motor_speed_mutex = new Mutex(this);
102 motor_speed = 0;
103 ExtraDraw();
104}
105
106void Plane::AnimateModel(void) {
107 motor_speed_mutex->GetMutex();
108 motor_speed=10;
109 f_blade->SetRotationSpeed(K_MOT *vector3df( 0,motor_speed,0));
110
111 motor_speed_mutex->ReleaseMutex();
112
113 // adapt UAV size
114 /*
115 if (arm_length->ValueChanged() == true) {
116 setScale(arm_length->Value());
117 }*/
118}
119
120size_t Plane::dbtSize(void) const {
121 return 6 * sizeof(float) + 1 * sizeof(float); // 6ddl+1moteur
122}
123
124void Plane::WritedbtBuf(
125 char *dbtbuf) { /*
126 float *buf=(float*)dbtbuf;
127 vector3df vect=getPosition();
128 memcpy(buf,&vect.X,sizeof(float));
129 buf++;
130 memcpy(buf,&vect.Y,sizeof(float));
131 buf++;
132 memcpy(buf,&vect.Z,sizeof(float));
133 buf++;
134 vect=getRotation();
135 memcpy(buf,&vect.X,sizeof(float));
136 buf++;
137 memcpy(buf,&vect.Y,sizeof(float));
138 buf++;
139 memcpy(buf,&vect.Z,sizeof(float));
140 buf++;
141 memcpy(buf,&motors,sizeof(rtsimu_motors));*/
142}
143
144void Plane::ReaddbtBuf(
145 char *dbtbuf) { /*
146 float *buf=(float*)dbtbuf;
147 vector3df vect;
148 memcpy(&vect.X,buf,sizeof(float));
149 buf++;
150 memcpy(&vect.Y,buf,sizeof(float));
151 buf++;
152 memcpy(&vect.Z,buf,sizeof(float));
153 buf++;
154 setPosition(vect);
155 memcpy(&vect.X,buf,sizeof(float));
156 buf++;
157 memcpy(&vect.Y,buf,sizeof(float));
158 buf++;
159 memcpy(&vect.Z,buf,sizeof(float));
160 buf++;
161 ((ISceneNode*)(this))->setRotation(vect);
162 memcpy(&motors,buf,sizeof(rtsimu_motors));
163 AnimateModele();*/
164}
165#endif // GL
166
167// states are computed on fixed frame NED
168// x north
169// y east
170// z down
171void Plane::CalcModel(void) {
172
173 float u_roll, u_pitch, u_yaw, u_thrust;
174 Time motorTime;
175#ifdef GL
176 motor_speed_mutex->GetMutex();
177#endif // GL
178 motor->GetSpeeds(&motor_speed,&motorTime);
179 if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
180 if(motor_speed!=0) {
181 //Printf("timout\n");
182 motor_speed=0;
183 }
184 }
185#ifdef GL
186 motor_speed_mutex->ReleaseMutex();
187#endif // GL
188
189
190 /*
191 ** ===================================================================
192 ** u roll: roll torque
193 **
194 ** ===================================================================
195 */
196 /*
197 u_roll = arm_length->Value() * k_mot->Value() *
198 (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
199 rr_speed * rr_speed) *
200 sqrtf(2) / 2;
201
202 /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
203 /// of the quadri in the body frame). It is a discrete integrator
204 state[0].W.x =
205 (dT() / j_roll->Value()) *
206 ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
207 u_roll) +
208 state[-1].W.x;
209*/
210
211 /*
212 ** ===================================================================
213 ** u pitch : pitch torque
214 **
215 ** ===================================================================
216 */
217 /*
218 u_pitch = arm_length->Value() * k_mot->Value() *
219 (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
220 rr_speed * rr_speed) *
221 sqrtf(2) / 2;
222
223 /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
224 /// of the quadri in the body frame). It is a discrete integrator
225 state[0].W.y =
226 (dT() / j_pitch->Value()) *
227 ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
228 u_pitch) +
229 state[-1].W.y;
230*/
231
232 /*
233 ** ===================================================================
234 ** u yaw : yaw torque
235 **
236 ** ===================================================================
237 */
238 /*
239 u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
240 fr_speed * fr_speed - rl_speed * rl_speed);
241
242 /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
243 /// of the quadri in the body frame). It is a discrete integrator
244 state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
245*/
246
247 // compute quaternion from W
248 // Quaternion derivative: dQ = 0.5*(Q*Qw)
249 Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
250
251 // Quaternion integration
252 state[0].Quat = state[-1].Quat + dQ * dT();
253 state[0].Quat.Normalize();
254
255 // Calculation of the thrust from the reference speed of motors
256 /*
257 u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
258 rl_speed * rl_speed + rr_speed * rr_speed);
259 Vector3D<double> vect(0, 0, -u_thrust);
260 vect.Rotate(state[0].Quat);
261*/
262 /*
263 ** ===================================================================
264 ** x double integrator
265 **
266 ** ===================================================================
267 */
268 /*
269 state[0].Pos.x =
270 (dT() * dT() / m->Value()) *
271 (vect.x - f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
272 2 * state[-1].Pos.x - state[-2].Pos.x;
273 state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
274 */
275 /*
276 ** ===================================================================
277 ** y double integrator
278 **
279 ** ===================================================================
280 */
281 /*
282 state[0].Pos.y =
283 (dT() * dT() / m->Value()) *
284 (vect.y -
285 f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
286 2 * state[-1].Pos.y - state[-2].Pos.y;
287 state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
288*/
289 /*
290 ** ===================================================================
291 ** z double integrator
292 **
293 ** ===================================================================
294 */
295 /*
296 state[0].Pos.z =
297 (dT() * dT() / m->Value()) *
298 (vect.z +
299 f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
300 m->Value() * G) +
301 2 * state[-1].Pos.z - state[-2].Pos.z;
302 state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
303*/
304#ifndef GL
305 if (state[0].Pos.z < 0)
306 state[0].Pos.z = 0;
307#endif
308}
309
310} // end namespace simulator
311} // end namespace flair
Note: See TracBrowser for help on using the repository browser.