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

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

simulator

File size: 10.3 KB
Line 
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
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
46X4::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
63X4::~X4()
64{
65    //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
66}
67
68
69#ifdef GL
70
71void 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
107void 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
123size_t X4::dbtSize(void) const
124{
125    return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
126}
127
128void 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
148void 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
175void 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
Note: See TracBrowser for help on using the repository browser.