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