Changeset 15 in flair-src for trunk/lib/FlairSimulator/src/X8.cpp


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSimulator/src/X8.cpp

    r10 r15  
    3131#endif
    3232
    33 #define K_MOT 0.4f //blade animation
    34 #define G (float)9.81 //gravity ( N/(m/s²) )
     33#define K_MOT 0.4f    // blade animation
     34#define G (float)9.81 // gravity ( N/(m/s²) )
    3535
    3636#ifdef GL
    … …  
    4343using namespace flair::actuator;
    4444
    45 namespace flair
    46 {
    47 namespace simulator
    48 {
    49 
    50 X8::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 
    70 void 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 
    119 X8::~X8()
    120 {
    121     //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
    122 }
    123 
    124 #ifdef GL
    125 void 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 
    146 size_t X8::dbtSize(void) const
    147 {
    148     return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
    149 }
    150 
    151 void 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 
    171 void 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
    198 void 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();
     45namespace flair {
     46namespace simulator {
     47
     48X8::X8(const Simulator *parent, std::string name, int dev_id)
     49    : Model(parent, name) {
     50  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     51  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
     52  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
     53                                 0, 2, 0.1);
     54  l_cg = new DoubleSpinBox(
     55      setup_tab->LastRowLastCol(), "position G (m):", -0.5, 0.5,
     56      0.02); // position du centre de gravité/centre de poussé
     57  k_mot =
     58      new DoubleSpinBox(setup_tab->NewRow(), "k_mot:", 0, 1, 0.001,
     59                        3); // vitesse rotation² (unité arbitraire) -> force (N)
     60  c_mot = new DoubleSpinBox(
     61      setup_tab->LastRowLastCol(), "c_mot:", 0, 1, 0.001,
     62      3); // vitesse rotation moteur -> couple (N.m/unité arbitraire)
     63  f_air_vert = new DoubleSpinBox(setup_tab->NewRow(), "f_air_vert:", 0, 10,
     64                                 1); // frottements air depl. vertical, aussi
     65                                     // utilisé pour les rotations ( N/(m/s) )
     66                                     // (du aux helices en rotation)
     67  f_air_lat =
     68      new DoubleSpinBox(setup_tab->LastRowLastCol(), "f_air_lat:", 0, 10,
     69                        1); // frottements air deplacements lateraux ( N/(m/s) )
     70  j_roll = new DoubleSpinBox(setup_tab->NewRow(), "j_roll:", 0, 1, 0.001,
     71                             5); // moment d'inertie d'un axe (N.m.s²/rad)
     72  j_pitch =
     73      new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_pitch:", 0, 1, 0.001,
     74                        5); // moment d'inertie d'un axe (N.m.s²/rad)
     75  j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
     76                            5); // moment d'inertie d'un axe (N.m.s²/rad)
     77  j_r = new DoubleSpinBox(setup_tab->NewRow(), "j_r:", 0, 1,
     78                          0.001); // moment des helices (N.m.s²/rad)
     79  sigma = new DoubleSpinBox(
     80      setup_tab->LastRowLastCol(), "sigma:", 0, 1,
     81      0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
     82  S = new DoubleSpinBox(
     83      setup_tab->LastRowLastCol(), "S:", 1, 2,
     84      0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
     85
     86  motors = new SimuBldc(this, name, 8, dev_id);
     87}
     88
     89void X8::Draw() {
     90#ifdef GL
     91
     92  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
     93  // parameter
     94  // note that the frame used is irrlicht one:
     95  // left handed, North East Up
     96
     97  const IGeometryCreator *geo;
     98  geo = getGui()->getSceneManager()->getGeometryCreator();
     99
     100  // cylinders are aligned with y axis
     101  red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
     102  black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
     103  motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
     104  // geo->drop();
     105
     106  ITexture *texture = getGui()->getTexture("carbone.jpg");
     107  fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     108                             vector3df(0, 0, -135));
     109  fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     110                             vector3df(0, 0, -45));
     111  rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     112                             vector3df(0, 0, 135), texture);
     113  rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     114                             vector3df(0, 0, 45), texture);
     115
     116  texture = getGui()->getTexture("metal047.jpg");
     117  tfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
     118                                vector3df(90, 0, 0), texture);
     119  tfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
     120                                vector3df(90, 0, 0), texture);
     121  trl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
     122                                vector3df(90, 0, 0), texture);
     123  trr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
     124                                vector3df(90, 0, 0), texture);
     125
     126  bfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, -17.5),
     127                                vector3df(90, 0, 0), texture);
     128  bfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, -17.5),
     129                                vector3df(90, 0, 0), texture);
     130  brl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, -17.5),
     131                                vector3df(90, 0, 0), texture);
     132  brr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, -17.5),
     133                                vector3df(90, 0, 0), texture);
     134
     135  tfl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
     136  tfr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
     137  trl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
     138  trr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
     139
     140  bfl_blade = new Blade(this, vector3df(70.71, -70.71, -17.5));
     141  bfr_blade = new Blade(this, vector3df(70.71, 70.71, -17.5), true);
     142  brl_blade = new Blade(this, vector3df(-70.71, -70.71, -17.5), true);
     143  brr_blade = new Blade(this, vector3df(-70.71, 70.71, -17.5));
     144
     145  motor_speed_mutex = new Mutex(this);
     146  for (int i = 0; i < 8; i++)
     147    motor_speed[i] = 0;
     148  ExtraDraw();
     149#endif
     150}
     151
     152X8::~X8() {
     153  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
     154  // pales) par parenté
     155}
     156
     157#ifdef GL
     158void X8::AnimateModel(void) {
     159  motor_speed_mutex->GetMutex();
     160  tfl_blade->SetRotationSpeed(K_MOT * motor_speed[0]);
     161  tfr_blade->SetRotationSpeed(-K_MOT * motor_speed[1]);
     162  trl_blade->SetRotationSpeed(-K_MOT * motor_speed[2]);
     163  trr_blade->SetRotationSpeed(K_MOT * motor_speed[3]);
     164
     165  bfl_blade->SetRotationSpeed(-K_MOT * motor_speed[4]);
     166  bfr_blade->SetRotationSpeed(K_MOT * motor_speed[5]);
     167  brl_blade->SetRotationSpeed(K_MOT * motor_speed[6]);
     168  brr_blade->SetRotationSpeed(-K_MOT * motor_speed[7]);
     169  motor_speed_mutex->ReleaseMutex();
     170
     171  // adapt UAV size
     172  if (arm_length->ValueChanged() == true) {
     173    setScale(arm_length->Value());
     174  }
     175}
     176
     177size_t X8::dbtSize(void) const {
     178  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
     179}
     180
     181void X8::WritedbtBuf(
     182    char *dbtbuf) { /*
     183                       float *buf=(float*)dbtbuf;
     184                       vector3df vect=getPosition();
     185                       memcpy(buf,&vect.X,sizeof(float));
     186                       buf++;
     187                       memcpy(buf,&vect.Y,sizeof(float));
     188                       buf++;
     189                       memcpy(buf,&vect.Z,sizeof(float));
     190                       buf++;
     191                       vect=getRotation();
     192                       memcpy(buf,&vect.X,sizeof(float));
     193                       buf++;
     194                       memcpy(buf,&vect.Y,sizeof(float));
     195                       buf++;
     196                       memcpy(buf,&vect.Z,sizeof(float));
     197                       buf++;
     198                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
     199}
     200
     201void X8::ReaddbtBuf(
     202    char *dbtbuf) { /*
     203                       float *buf=(float*)dbtbuf;
     204                       vector3df vect;
     205                       memcpy(&vect.X,buf,sizeof(float));
     206                       buf++;
     207                       memcpy(&vect.Y,buf,sizeof(float));
     208                       buf++;
     209                       memcpy(&vect.Z,buf,sizeof(float));
     210                       buf++;
     211                       setPosition(vect);
     212                       memcpy(&vect.X,buf,sizeof(float));
     213                       buf++;
     214                       memcpy(&vect.Y,buf,sizeof(float));
     215                       buf++;
     216                       memcpy(&vect.Z,buf,sizeof(float));
     217                       buf++;
     218                       ((ISceneNode*)(this))->setRotation(vect);
     219                       memcpy(&motors,buf,sizeof(rtsimu_motors));
     220                       AnimateModele();*/
     221}
     222#endif // GL
     223
     224// states are computed on fixed frame NED
     225// x north
     226// y east
     227// z down
     228void X8::CalcModel(void) {
     229  float tfl_speed, tfr_speed, trl_speed, trr_speed;
     230  float bfl_speed, bfr_speed, brl_speed, brr_speed;
     231  float u_roll, u_pitch, u_yaw, u_thrust;
     232  float omega;
     233#ifdef GL
     234  motor_speed_mutex->GetMutex();
     235#endif // GL
     236  motors->GetSpeeds(motor_speed);
     237#ifdef GL
     238  motor_speed_mutex->ReleaseMutex();
     239#endif // GL
     240  tfl_speed = motor_speed[0];
     241  tfr_speed = motor_speed[1];
     242  trl_speed = motor_speed[2];
     243  trr_speed = motor_speed[3];
     244  bfl_speed = motor_speed[4];
     245  bfr_speed = motor_speed[5];
     246  brl_speed = motor_speed[6];
     247  brr_speed = motor_speed[7];
     248
     249  omega = tfl_speed + brl_speed + trr_speed + bfr_speed - bfl_speed -
     250          trl_speed - brr_speed - tfr_speed;
     251
     252  /*
     253      ** ===================================================================
     254      **    u roll: roll torque
     255      **
     256      ** ===================================================================
     257      */
     258
     259  u_roll = arm_length->Value() * k_mot->Value() *
     260           (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     261            sigma->Value() * trl_speed * trl_speed + brl_speed * brl_speed -
     262            sigma->Value() * tfr_speed * tfr_speed - bfr_speed * bfr_speed -
     263            sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     264           sqrtf(2) / 2;
     265
     266  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
     267  /// of the quadri in the body frame). It is a discrete integrator
     268  // 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
     269  // + u_roll) +state[-1].W.x;//Osamah
     270  state[0].W.x =
     271      (dT() / j_roll->Value()) *
     272          ((j_pitch->Value() - j_yaw->Value()) * state[-1].W.y * state[-1].W.z -
     273           j_r->Value() * state[-1].W.y * omega + u_roll) +
     274      state[-1].W.x; // Majd
     275
     276  // 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;
     277
     278  /*
     279  ** ===================================================================
     280  **   u pitch : pitch torque
     281  **
     282  ** ===================================================================
     283  */
     284  u_pitch = arm_length->Value() * k_mot->Value() *
     285            (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     286             sigma->Value() * tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     287             sigma->Value() * trl_speed * trl_speed - brl_speed * brl_speed -
     288             sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     289            sqrtf(2) / 2;
     290
     291  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
     292  /// of the quadri in the body frame). It is a discrete integrator
     293  // 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
     294  // + u_pitch)+state[-1].W.y;//Osamah
     295  state[0].W.y =
     296      (dT() / j_pitch->Value()) *
     297          ((j_yaw->Value() - j_roll->Value()) * state[-1].W.x * state[-1].W.z -
     298           j_r->Value() * state[-1].W.x * omega + u_pitch) +
     299      state[-1].W.y; // Majd
     300
     301  // 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;
     302
     303  /*
     304  ** ===================================================================
     305  **    u yaw : yaw torque
     306  **
     307  ** ===================================================================
     308  */
     309  u_yaw = c_mot->Value() * (tfl_speed * tfl_speed - bfl_speed * bfl_speed +
     310                            trr_speed * trr_speed - brr_speed * brr_speed -
     311                            tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     312                            trl_speed * trl_speed + brl_speed * brl_speed);
     313
     314  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
     315  /// of the quadri in the body frame). It is a discrete integrator
     316  // state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
     317  state[0].W.z =
     318      (dT() / j_yaw->Value()) * ((j_roll->Value() - j_pitch->Value()) *
     319                                     state[-1].W.x * state[-1].W.y +
     320                                 u_yaw) +
     321      state[-1].W.z; // Majd
     322
     323  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
     324
     325  // compute quaternion from W
     326  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     327  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     328
     329  // Quaternion integration
     330  state[0].Quat = state[-1].Quat + dQ * dT();
     331  state[0].Quat.Normalize();
     332
     333  // Calculation of the thrust from the reference speed of motors
     334  u_thrust =
     335      k_mot->Value() * S->Value() *
     336      (sigma->Value() * tfl_speed * tfl_speed +
     337       sigma->Value() * tfr_speed * tfr_speed +
     338       sigma->Value() * trl_speed * trl_speed +
     339       sigma->Value() * trr_speed * trr_speed + bfl_speed * bfl_speed +
     340       bfr_speed * bfr_speed + brl_speed * brl_speed + brr_speed * brr_speed);
     341  Vector3D vect(0, 0, -u_thrust);
     342  vect.Rotate(state[0].Quat);
     343
     344  /*
     345      ** ===================================================================
     346      **     x double integrator
     347      **
     348      ** ===================================================================
     349      */
     350  state[0].Pos.x =
     351      (dT() * dT() / m->Value()) *
     352          (vect.x -
     353           f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
     354      2 * state[-1].Pos.x - state[-2].Pos.x;
     355  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
     356
     357  /*
     358      ** ===================================================================
     359      **     y double integrator
     360      **
     361      ** ===================================================================
     362      */
     363  state[0].Pos.y =
     364      (dT() * dT() / m->Value()) *
     365          (vect.y -
     366           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
     367      2 * state[-1].Pos.y - state[-2].Pos.y;
     368  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     369
     370  /*
     371  ** ===================================================================
     372  **     z double integrator
     373  **
     374  ** ===================================================================
     375  */
     376  state[0].Pos.z =
     377      (dT() * dT() / m->Value()) *
     378          (vect.z +
     379           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
     380           m->Value() * G) +
     381      2 * state[-1].Pos.z - state[-2].Pos.z;
     382  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    316383
    317384#ifndef GL
    318     if(state[0].Pos.z<0) state[0].Pos.z=0;
     385  if (state[0].Pos.z < 0)
     386    state[0].Pos.z = 0;
    319387#endif
    320 
    321388}
    322389
Note: See TracChangeset for help on using the changeset viewer.