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

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

lic

File size: 13.1 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:    2014/04/03
6//  filename:   X8.cpp
7//
8//  author:     Majd Saied, Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    classe definissant un X8
14//
15/*********************************************************************/
16
17#include "X8.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
50X8::X8(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->LastRowLastCol(),"position G (m):",-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        j_r=new DoubleSpinBox(setup_tab->NewRow(),"j_r:",0,1,0.001);// moment des helices (N.m.s²/rad)
64        sigma=new DoubleSpinBox(setup_tab->LastRowLastCol(),"sigma:",0,1,0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
65        S=new DoubleSpinBox(setup_tab->LastRowLastCol(),"S:",1,2,0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
66
67    motors=new SimuBldc(this,name,8,dev_id);
68}
69
70void X8::Draw(){
71#ifdef GL
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
77    const IGeometryCreator *geo;
78    geo=getGui()->getSceneManager()->getGeometryCreator();
79
80    //cylinders are aligned with y axis
81    red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
82    black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
83    motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
84    //geo->drop();
85
86    ITexture* texture=getGui()->getTexture("carbone.jpg");
87    fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
88    fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
89    rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
90    rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
91
92    texture=getGui()->getTexture("metal047.jpg");
93    tfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
94    tfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
95    trl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
96    trr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
97
98    bfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,-17.5),vector3df(90,0,0),texture);
99    bfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,-17.5),vector3df(90,0,0),texture);
100    brl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,-17.5),vector3df(90,0,0),texture);
101    brr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,-17.5),vector3df(90,0,0),texture);
102
103    tfl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
104    tfr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
105    trl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
106    trr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
107
108    bfl_blade=new Blade(this, vector3df(70.71,-70.71,-17.5));
109    bfr_blade=new Blade(this, vector3df(70.71,70.71,-17.5),true);
110    brl_blade=new Blade(this, vector3df(-70.71,-70.71,-17.5),true);
111    brr_blade=new Blade(this, vector3df(-70.71,70.71,-17.5));
112
113    motor_speed_mutex=new Mutex(this);
114    for(int i=0;i<8;i++) motor_speed[i]=0;
115    ExtraDraw();
116    #endif
117}
118
119X8::~X8()
120{
121    //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
122}
123
124#ifdef GL
125void X8::AnimateModel(void)
126{
127    motor_speed_mutex->GetMutex();
128    tfl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
129    tfr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
130    trl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
131    trr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
132
133    bfl_blade->SetRotationSpeed(-K_MOT*motor_speed[4]);
134    bfr_blade->SetRotationSpeed(K_MOT*motor_speed[5]);
135    brl_blade->SetRotationSpeed(K_MOT*motor_speed[6]);
136    brr_blade->SetRotationSpeed(-K_MOT*motor_speed[7]);
137    motor_speed_mutex->ReleaseMutex();
138
139    //adapt UAV size
140    if(arm_length->ValueChanged()==true)
141    {
142        setScale(arm_length->Value());
143    }
144}
145
146size_t X8::dbtSize(void) const
147{
148    return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
149}
150
151void X8::WritedbtBuf(char* dbtbuf)
152{/*
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
171void X8::ReaddbtBuf(char* dbtbuf)
172{/*
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
198void X8::CalcModel(void)
199{
200    float tfl_speed,tfr_speed,trl_speed,trr_speed;
201    float bfl_speed,bfr_speed,brl_speed,brr_speed;
202    float u_roll,u_pitch,u_yaw,u_thrust;
203    float omega;
204#ifdef GL
205    motor_speed_mutex->GetMutex();
206#endif //GL
207    motors->GetSpeeds(motor_speed);
208#ifdef GL
209    motor_speed_mutex->ReleaseMutex();
210#endif //GL
211        tfl_speed=motor_speed[0];
212        tfr_speed=motor_speed[1];
213        trl_speed=motor_speed[2];
214        trr_speed=motor_speed[3];
215        bfl_speed=motor_speed[4];
216        bfr_speed=motor_speed[5];
217        brl_speed=motor_speed[6];
218        brr_speed=motor_speed[7];
219
220        omega=tfl_speed+brl_speed+trr_speed+bfr_speed-bfl_speed-trl_speed-brr_speed-tfr_speed;
221
222
223    /*
224        ** ===================================================================
225        **    u roll: roll torque
226        **
227        ** ===================================================================
228        */
229
230    u_roll=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
231                                                +sigma->Value()*trl_speed*trl_speed+brl_speed*brl_speed
232                                                -sigma->Value()*tfr_speed*tfr_speed-bfr_speed*bfr_speed
233                                                -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
234
235    /// 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
236        //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
237    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
238
239        //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;
240
241        /*
242        ** ===================================================================
243        **   u pitch : pitch torque
244        **
245        ** ===================================================================
246        */
247        u_pitch=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
248                                             +sigma->Value()*tfr_speed*tfr_speed+bfr_speed*bfr_speed
249                                            -sigma->Value()*trl_speed*trl_speed-brl_speed*brl_speed
250                                             -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
251
252    /// 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
253        //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
254    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
255
256   //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;
257
258        /*
259        ** ===================================================================
260        **    u yaw : yaw torque
261        **
262        ** ===================================================================
263        */
264        u_yaw=c_mot->Value()*(tfl_speed*tfl_speed-bfl_speed*bfl_speed
265                        +trr_speed*trr_speed-brr_speed*brr_speed
266                        -tfr_speed*tfr_speed+bfr_speed*bfr_speed
267                        -trl_speed*trl_speed+brl_speed*brl_speed);
268
269    /// 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
270        //state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
271        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
272
273        //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
274
275    // compute quaternion from W
276    // Quaternion derivative: dQ = 0.5*(Q*Qw)
277    Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
278
279    // Quaternion integration
280    state[0].Quat =state[-1].Quat +dQ*dT();
281    state[0].Quat.Normalize();
282
283    // Calculation of the thrust from the reference speed of motors
284    u_thrust=k_mot->Value()*S->Value()*
285       (sigma->Value()*tfl_speed*tfl_speed+sigma->Value()*tfr_speed*tfr_speed+sigma->Value()*trl_speed*trl_speed+sigma->Value()*trr_speed*trr_speed
286       +bfl_speed*bfl_speed+bfr_speed*bfr_speed+brl_speed*brl_speed+brr_speed*brr_speed);
287    Vector3D vect(0,0,-u_thrust);
288    vect.Rotate(state[0].Quat);
289
290    /*
291        ** ===================================================================
292        **     x double integrator
293        **
294        ** ===================================================================
295        */
296        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;
297    state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
298
299    /*
300        ** ===================================================================
301        **     y double integrator
302        **
303        ** ===================================================================
304        */
305        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;
306    state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
307
308        /*
309        ** ===================================================================
310        **     z double integrator
311        **
312        ** ===================================================================
313        */
314        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;
315    state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
316
317#ifndef GL
318    if(state[0].Pos.z<0) state[0].Pos.z=0;
319#endif
320
321}
322
323} // end namespace simulator
324} // end namespace flair
Note: See TracBrowser for help on using the repository browser.