source: flair-src/trunk/lib/FlairSimulator/src/X8.cpp@ 9

Last change on this file since 9 was 8, checked in by Sanahuja Guillaume, 9 years ago

simulator
File size: 12.9 KB
RevLine 
[8]1// created: 2014/04/03
2// filename: X8.cpp
3//
4// author: Majd Saied, Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: classe definissant un X8
10//
11/*********************************************************************/
12
13#include "X8.h"
14#include "Simulator.h"
15#include <SimuBldc.h>
16#include <TabWidget.h>
17#include <Tab.h>
18#include <DoubleSpinBox.h>
19#include <GroupBox.h>
20#include <math.h>
21#ifdef GL
22#include <ISceneManager.h>
23#include "Blade.h"
24#include "MeshSceneNode.h"
25#include "Gui.h"
26#include <Mutex.h>
27#endif
28
29#define K_MOT 0.4f //blade animation
30#define G (float)9.81 //gravity ( N/(m/s²) )
31
32#ifdef GL
33using namespace irr::video;
34using namespace irr::scene;
35using namespace irr::core;
36#endif
37using namespace flair::core;
38using namespace flair::gui;
39using namespace flair::actuator;
40
41namespace flair
42{
43namespace simulator
44{
45
46X8::X8(const Simulator* parent,std::string name, int dev_id): Model(parent,name)
47{
48 Tab *setup_tab=new Tab(GetTabWidget(),"model");
49 m=new DoubleSpinBox(setup_tab->NewRow(),"mass (kg):",0,20,0.1);
50 arm_length=new DoubleSpinBox(setup_tab->LastRowLastCol(),"arm length (m):",0,2,0.1);
51 l_cg=new DoubleSpinBox(setup_tab->LastRowLastCol(),"position G (m):",-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
52 k_mot=new DoubleSpinBox(setup_tab->NewRow(),"k_mot:",0,1,0.001,3);// vitesse rotation² (unité arbitraire) -> force (N)
53 c_mot=new DoubleSpinBox(setup_tab->LastRowLastCol(),"c_mot:",0,1,0.001,3);// vitesse rotation moteur -> couple (N.m/unité arbitraire)
54 f_air_vert=new DoubleSpinBox(setup_tab->NewRow(),"f_air_vert:",0,10,1);//frottements air depl. vertical, aussi utilisé pour les rotations ( N/(m/s) ) (du aux helices en rotation)
55 f_air_lat=new DoubleSpinBox(setup_tab->LastRowLastCol(),"f_air_lat:",0,10,1);//frottements air deplacements lateraux ( N/(m/s) )
56 j_roll=new DoubleSpinBox(setup_tab->NewRow(),"j_roll:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
57 j_pitch=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_pitch:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
58 j_yaw=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_yaw:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
59 j_r=new DoubleSpinBox(setup_tab->NewRow(),"j_r:",0,1,0.001);// moment des helices (N.m.s²/rad)
60 sigma=new DoubleSpinBox(setup_tab->LastRowLastCol(),"sigma:",0,1,0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
61 S=new DoubleSpinBox(setup_tab->LastRowLastCol(),"S:",1,2,0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
62
63 motors=new SimuBldc(this,name,8,dev_id);
64}
65
66void X8::Draw(){
67#ifdef GL
68
69 //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
70 //note that the frame used is irrlicht one:
71 //left handed, North East Up
72
73 const IGeometryCreator *geo;
74 geo=getGui()->getSceneManager()->getGeometryCreator();
75
76 //cylinders are aligned with y axis
77 red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
78 black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
79 motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
80 //geo->drop();
81
82 ITexture* texture=getGui()->getTexture("carbone.jpg");
83 fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
84 fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
85 rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
86 rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
87
88 texture=getGui()->getTexture("metal047.jpg");
89 tfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
90 tfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
91 trl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
92 trr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
93
94 bfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,-17.5),vector3df(90,0,0),texture);
95 bfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,-17.5),vector3df(90,0,0),texture);
96 brl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,-17.5),vector3df(90,0,0),texture);
97 brr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,-17.5),vector3df(90,0,0),texture);
98
99 tfl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
100 tfr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
101 trl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
102 trr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
103
104 bfl_blade=new Blade(this, vector3df(70.71,-70.71,-17.5));
105 bfr_blade=new Blade(this, vector3df(70.71,70.71,-17.5),true);
106 brl_blade=new Blade(this, vector3df(-70.71,-70.71,-17.5),true);
107 brr_blade=new Blade(this, vector3df(-70.71,70.71,-17.5));
108
109 motor_speed_mutex=new Mutex(this);
110 for(int i=0;i<8;i++) motor_speed[i]=0;
111 ExtraDraw();
112 #endif
113}
114
115X8::~X8()
116{
117 //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
118}
119
120#ifdef GL
121void X8::AnimateModel(void)
122{
123 motor_speed_mutex->GetMutex();
124 tfl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
125 tfr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
126 trl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
127 trr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
128
129 bfl_blade->SetRotationSpeed(-K_MOT*motor_speed[4]);
130 bfr_blade->SetRotationSpeed(K_MOT*motor_speed[5]);
131 brl_blade->SetRotationSpeed(K_MOT*motor_speed[6]);
132 brr_blade->SetRotationSpeed(-K_MOT*motor_speed[7]);
133 motor_speed_mutex->ReleaseMutex();
134
135 //adapt UAV size
136 if(arm_length->ValueChanged()==true)
137 {
138 setScale(arm_length->Value());
139 }
140}
141
142size_t X8::dbtSize(void) const
143{
144 return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
145}
146
147void X8::WritedbtBuf(char* dbtbuf)
148{/*
149 float *buf=(float*)dbtbuf;
150 vector3df vect=getPosition();
151 memcpy(buf,&vect.X,sizeof(float));
152 buf++;
153 memcpy(buf,&vect.Y,sizeof(float));
154 buf++;
155 memcpy(buf,&vect.Z,sizeof(float));
156 buf++;
157 vect=getRotation();
158 memcpy(buf,&vect.X,sizeof(float));
159 buf++;
160 memcpy(buf,&vect.Y,sizeof(float));
161 buf++;
162 memcpy(buf,&vect.Z,sizeof(float));
163 buf++;
164 memcpy(buf,&motors,sizeof(rtsimu_motors));*/
165}
166
167void X8::ReaddbtBuf(char* dbtbuf)
168{/*
169 float *buf=(float*)dbtbuf;
170 vector3df vect;
171 memcpy(&vect.X,buf,sizeof(float));
172 buf++;
173 memcpy(&vect.Y,buf,sizeof(float));
174 buf++;
175 memcpy(&vect.Z,buf,sizeof(float));
176 buf++;
177 setPosition(vect);
178 memcpy(&vect.X,buf,sizeof(float));
179 buf++;
180 memcpy(&vect.Y,buf,sizeof(float));
181 buf++;
182 memcpy(&vect.Z,buf,sizeof(float));
183 buf++;
184 ((ISceneNode*)(this))->setRotation(vect);
185 memcpy(&motors,buf,sizeof(rtsimu_motors));
186 AnimateModele();*/
187}
188#endif //GL
189
190//states are computed on fixed frame NED
191//x north
192//y east
193//z down
194void X8::CalcModel(void)
195{
196 float tfl_speed,tfr_speed,trl_speed,trr_speed;
197 float bfl_speed,bfr_speed,brl_speed,brr_speed;
198 float u_roll,u_pitch,u_yaw,u_thrust;
199 float omega;
200#ifdef GL
201 motor_speed_mutex->GetMutex();
202#endif //GL
203 motors->GetSpeeds(motor_speed);
204#ifdef GL
205 motor_speed_mutex->ReleaseMutex();
206#endif //GL
207 tfl_speed=motor_speed[0];
208 tfr_speed=motor_speed[1];
209 trl_speed=motor_speed[2];
210 trr_speed=motor_speed[3];
211 bfl_speed=motor_speed[4];
212 bfr_speed=motor_speed[5];
213 brl_speed=motor_speed[6];
214 brr_speed=motor_speed[7];
215
216 omega=tfl_speed+brl_speed+trr_speed+bfr_speed-bfl_speed-trl_speed-brr_speed-tfr_speed;
217
218
219 /*
220 ** ===================================================================
221 ** u roll: roll torque
222 **
223 ** ===================================================================
224 */
225
226 u_roll=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
227 +sigma->Value()*trl_speed*trl_speed+brl_speed*brl_speed
228 -sigma->Value()*tfr_speed*tfr_speed-bfr_speed*bfr_speed
229 -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
230
231 /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed of the quadri in the body frame). It is a discrete integrator
232 //state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Osamah
233 state[0].W.x=(dT()/j_roll->Value())*((j_pitch->Value()-j_yaw->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Majd
234
235 //state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
236
237 /*
238 ** ===================================================================
239 ** u pitch : pitch torque
240 **
241 ** ===================================================================
242 */
243 u_pitch=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
244 +sigma->Value()*tfr_speed*tfr_speed+bfr_speed*bfr_speed
245 -sigma->Value()*trl_speed*trl_speed-brl_speed*brl_speed
246 -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
247
248 /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed of the quadri in the body frame). It is a discrete integrator
249 //state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Osamah
250 state[0].W.y=(dT()/j_pitch->Value())*((j_yaw->Value()-j_roll->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Majd
251
252 //state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
253
254 /*
255 ** ===================================================================
256 ** u yaw : yaw torque
257 **
258 ** ===================================================================
259 */
260 u_yaw=c_mot->Value()*(tfl_speed*tfl_speed-bfl_speed*bfl_speed
261 +trr_speed*trr_speed-brr_speed*brr_speed
262 -tfr_speed*tfr_speed+bfr_speed*bfr_speed
263 -trl_speed*trl_speed+brl_speed*brl_speed);
264
265 /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed of the quadri in the body frame). It is a discrete integrator
266 //state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
267 state[0].W.z=(dT()/j_yaw->Value())*((j_roll->Value()-j_pitch->Value())*state[-1].W.x*state[-1].W.y+u_yaw )+state[-1].W.z;//Majd
268
269 //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
270
271 // compute quaternion from W
272 // Quaternion derivative: dQ = 0.5*(Q*Qw)
273 Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
274
275 // Quaternion integration
276 state[0].Quat =state[-1].Quat +dQ*dT();
277 state[0].Quat.Normalize();
278
279 // Calculation of the thrust from the reference speed of motors
280 u_thrust=k_mot->Value()*S->Value()*
281 (sigma->Value()*tfl_speed*tfl_speed+sigma->Value()*tfr_speed*tfr_speed+sigma->Value()*trl_speed*trl_speed+sigma->Value()*trr_speed*trr_speed
282 +bfl_speed*bfl_speed+bfr_speed*bfr_speed+brl_speed*brl_speed+brr_speed*brr_speed);
283 Vector3D vect(0,0,-u_thrust);
284 vect.Rotate(state[0].Quat);
285
286 /*
287 ** ===================================================================
288 ** x double integrator
289 **
290 ** ===================================================================
291 */
292 state[0].Pos.x=(dT()*dT()/m->Value())*(vect.x-f_air_lat->Value()*(state[-1].Pos.x-state[-2].Pos.x)/dT())+2*state[-1].Pos.x-state[-2].Pos.x;
293 state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
294
295 /*
296 ** ===================================================================
297 ** y double integrator
298 **
299 ** ===================================================================
300 */
301 state[0].Pos.y=(dT()*dT()/m->Value())*(vect.y-f_air_lat->Value()*(state[-1].Pos.y-state[-2].Pos.y)/dT())+2*state[-1].Pos.y-state[-2].Pos.y;
302 state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
303
304 /*
305 ** ===================================================================
306 ** z double integrator
307 **
308 ** ===================================================================
309 */
310 state[0].Pos.z=(dT()*dT()/m->Value())*(vect.z+f_air_vert->Value()*(state[-1].Pos.z-state[-2].Pos.z)/dT()+m->Value()*G)+2*state[-1].Pos.z-state[-2].Pos.z;
311 state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
312
313#ifndef GL
314 if(state[0].Pos.z<0) state[0].Pos.z=0;
315#endif
316
317}
318
319} // end namespace simulator
320} // end namespace flair
Note: See TracBrowser for help on using the repository browser.