source: flair-src/trunk/lib/FlairSimulator/src/X4.cpp@ 10

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

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