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


Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

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