1 | // created: 2012/08/21
|
---|
2 | // filename: X4.cpp
|
---|
3 | //
|
---|
4 | // author: Osamah Saif, Guillaume Sanahuja
|
---|
5 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
6 | //
|
---|
7 | // version: $Id: $
|
---|
8 | //
|
---|
9 | // purpose: classe definissant un x4
|
---|
10 | //
|
---|
11 | /*********************************************************************/
|
---|
12 |
|
---|
13 | #include "X4.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
|
---|
33 | using namespace irr::video;
|
---|
34 | using namespace irr::scene;
|
---|
35 | using namespace irr::core;
|
---|
36 | #endif
|
---|
37 | using namespace flair::core;
|
---|
38 | using namespace flair::gui;
|
---|
39 | using namespace flair::actuator;
|
---|
40 |
|
---|
41 | namespace flair
|
---|
42 | {
|
---|
43 | namespace simulator
|
---|
44 | {
|
---|
45 |
|
---|
46 | X4::X4(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,"position G (m):",0,2,-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 |
|
---|
60 | motors=new SimuBldc(this,name,4,dev_id);
|
---|
61 | }
|
---|
62 |
|
---|
63 | X4::~X4()
|
---|
64 | {
|
---|
65 | //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
|
---|
66 | }
|
---|
67 |
|
---|
68 |
|
---|
69 | #ifdef GL
|
---|
70 |
|
---|
71 | void X4::Draw(void)
|
---|
72 | {
|
---|
73 | //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
|
---|
74 | //note that the frame used is irrlicht one:
|
---|
75 | //left handed, North East Up
|
---|
76 | const IGeometryCreator *geo;
|
---|
77 | geo=getGui()->getSceneManager()->getGeometryCreator();
|
---|
78 |
|
---|
79 | //cylinders are aligned with y axis
|
---|
80 | red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
|
---|
81 | black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
|
---|
82 | motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
|
---|
83 | //geo->drop();
|
---|
84 |
|
---|
85 | ITexture* texture=getGui()->getTexture("carbone.jpg");
|
---|
86 | fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
|
---|
87 | fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
|
---|
88 | rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
|
---|
89 | rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
|
---|
90 |
|
---|
91 | texture=getGui()->getTexture("metal047.jpg");
|
---|
92 | fl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
|
---|
93 | fr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
|
---|
94 | rl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
|
---|
95 | rr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
|
---|
96 |
|
---|
97 | fl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
|
---|
98 | fr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
|
---|
99 | rl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
|
---|
100 | rr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
|
---|
101 |
|
---|
102 | motor_speed_mutex=new Mutex(this);
|
---|
103 | for(int i=0;i<4;i++) motor_speed[i]=0;
|
---|
104 | ExtraDraw();
|
---|
105 | }
|
---|
106 |
|
---|
107 | void X4::AnimateModel(void)
|
---|
108 | {
|
---|
109 | motor_speed_mutex->GetMutex();
|
---|
110 | fl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
|
---|
111 | fr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
|
---|
112 | rl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
|
---|
113 | rr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
|
---|
114 | motor_speed_mutex->ReleaseMutex();
|
---|
115 |
|
---|
116 | //adapt UAV size
|
---|
117 | if(arm_length->ValueChanged()==true)
|
---|
118 | {
|
---|
119 | setScale(arm_length->Value());
|
---|
120 | }
|
---|
121 | }
|
---|
122 |
|
---|
123 | size_t X4::dbtSize(void) const
|
---|
124 | {
|
---|
125 | return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
|
---|
126 | }
|
---|
127 |
|
---|
128 | void X4::WritedbtBuf(char* dbtbuf)
|
---|
129 | {/*
|
---|
130 | float *buf=(float*)dbtbuf;
|
---|
131 | vector3df vect=getPosition();
|
---|
132 | memcpy(buf,&vect.X,sizeof(float));
|
---|
133 | buf++;
|
---|
134 | memcpy(buf,&vect.Y,sizeof(float));
|
---|
135 | buf++;
|
---|
136 | memcpy(buf,&vect.Z,sizeof(float));
|
---|
137 | buf++;
|
---|
138 | vect=getRotation();
|
---|
139 | memcpy(buf,&vect.X,sizeof(float));
|
---|
140 | buf++;
|
---|
141 | memcpy(buf,&vect.Y,sizeof(float));
|
---|
142 | buf++;
|
---|
143 | memcpy(buf,&vect.Z,sizeof(float));
|
---|
144 | buf++;
|
---|
145 | memcpy(buf,&motors,sizeof(rtsimu_motors));*/
|
---|
146 | }
|
---|
147 |
|
---|
148 | void X4::ReaddbtBuf(char* dbtbuf)
|
---|
149 | {/*
|
---|
150 | float *buf=(float*)dbtbuf;
|
---|
151 | vector3df vect;
|
---|
152 | memcpy(&vect.X,buf,sizeof(float));
|
---|
153 | buf++;
|
---|
154 | memcpy(&vect.Y,buf,sizeof(float));
|
---|
155 | buf++;
|
---|
156 | memcpy(&vect.Z,buf,sizeof(float));
|
---|
157 | buf++;
|
---|
158 | setPosition(vect);
|
---|
159 | memcpy(&vect.X,buf,sizeof(float));
|
---|
160 | buf++;
|
---|
161 | memcpy(&vect.Y,buf,sizeof(float));
|
---|
162 | buf++;
|
---|
163 | memcpy(&vect.Z,buf,sizeof(float));
|
---|
164 | buf++;
|
---|
165 | ((ISceneNode*)(this))->setRotation(vect);
|
---|
166 | memcpy(&motors,buf,sizeof(rtsimu_motors));
|
---|
167 | AnimateModele();*/
|
---|
168 | }
|
---|
169 | #endif //GL
|
---|
170 |
|
---|
171 | //states are computed on fixed frame NED
|
---|
172 | //x north
|
---|
173 | //y east
|
---|
174 | //z down
|
---|
175 | void X4::CalcModel(void)
|
---|
176 | {
|
---|
177 | float fl_speed,fr_speed,rl_speed,rr_speed;
|
---|
178 | float u_roll,u_pitch,u_yaw,u_thrust;
|
---|
179 | #ifdef GL
|
---|
180 | motor_speed_mutex->GetMutex();
|
---|
181 | #endif //GL
|
---|
182 | motors->GetSpeeds(motor_speed);
|
---|
183 | #ifdef GL
|
---|
184 | motor_speed_mutex->ReleaseMutex();
|
---|
185 | #endif //GL
|
---|
186 | fl_speed=motor_speed[0];
|
---|
187 | fr_speed=motor_speed[1];
|
---|
188 | rl_speed=motor_speed[2];
|
---|
189 | rr_speed=motor_speed[3];
|
---|
190 |
|
---|
191 | /*
|
---|
192 | ** ===================================================================
|
---|
193 | ** u roll: roll torque
|
---|
194 | **
|
---|
195 | ** ===================================================================
|
---|
196 | */
|
---|
197 | u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
|
---|
198 |
|
---|
199 | /// 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
|
---|
200 | state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z + u_roll) +state[-1].W.x;
|
---|
201 |
|
---|
202 | //u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
|
---|
203 | //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;
|
---|
204 |
|
---|
205 | /*
|
---|
206 | ** ===================================================================
|
---|
207 | ** u pitch : pitch torque
|
---|
208 | **
|
---|
209 | ** ===================================================================
|
---|
210 | */
|
---|
211 | u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
|
---|
212 |
|
---|
213 | /// 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
|
---|
214 | state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z + u_pitch)+state[-1].W.y;
|
---|
215 |
|
---|
216 | //u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
|
---|
217 | //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;
|
---|
218 |
|
---|
219 | /*
|
---|
220 | ** ===================================================================
|
---|
221 | ** u yaw : yaw torque
|
---|
222 | **
|
---|
223 | ** ===================================================================
|
---|
224 | */
|
---|
225 | u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
|
---|
226 |
|
---|
227 | /// 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
|
---|
228 | state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;
|
---|
229 |
|
---|
230 | //u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
|
---|
231 | //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
|
---|
232 |
|
---|
233 | // compute quaternion from W
|
---|
234 | // Quaternion derivative: dQ = 0.5*(Q*Qw)
|
---|
235 | Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
|
---|
236 |
|
---|
237 | // Quaternion integration
|
---|
238 | state[0].Quat = state[-1].Quat +dQ*dT();
|
---|
239 | state[0].Quat.Normalize();
|
---|
240 |
|
---|
241 | // Calculation of the thrust from the reference speed of motors
|
---|
242 | u_thrust=k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed+rl_speed*rl_speed+rr_speed*rr_speed);
|
---|
243 | Vector3D vect(0,0,-u_thrust);
|
---|
244 | vect.Rotate(state[0].Quat);
|
---|
245 |
|
---|
246 | /*
|
---|
247 | ** ===================================================================
|
---|
248 | ** x double integrator
|
---|
249 | **
|
---|
250 | ** ===================================================================
|
---|
251 | */
|
---|
252 | 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;
|
---|
253 | state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
|
---|
254 |
|
---|
255 | /*
|
---|
256 | ** ===================================================================
|
---|
257 | ** y double integrator
|
---|
258 | **
|
---|
259 | ** ===================================================================
|
---|
260 | */
|
---|
261 | 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;
|
---|
262 | state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
|
---|
263 |
|
---|
264 | /*
|
---|
265 | ** ===================================================================
|
---|
266 | ** z double integrator
|
---|
267 | **
|
---|
268 | ** ===================================================================
|
---|
269 | */
|
---|
270 | 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;
|
---|
271 | state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
|
---|
272 |
|
---|
273 | #ifndef GL
|
---|
274 | if(state[0].Pos.z<0) state[0].Pos.z=0;
|
---|
275 | #endif
|
---|
276 |
|
---|
277 | }
|
---|
278 |
|
---|
279 | } // end namespace simulator
|
---|
280 | } // end namespace flair
|
---|