Changeset 15 in flair-src for trunk/lib/FlairSimulator/src/X8.cpp
- Timestamp:
- 04/08/16 15:40:57 (7 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSimulator/src/X8.cpp
r10 r15 31 31 #endif 32 32 33 #define K_MOT 0.4f //blade animation34 #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²) ) 35 35 36 36 #ifdef GL … … 43 43 using namespace flair::actuator; 44 44 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(); 45 namespace flair { 46 namespace simulator { 47 48 X8::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 89 void 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 152 X8::~X8() { 153 // les objets irrlicht seront automatiquement detruits (moteurs, helices, 154 // pales) par parenté 155 } 156 157 #ifdef GL 158 void 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 177 size_t X8::dbtSize(void) const { 178 return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices 179 } 180 181 void 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 201 void 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 228 void 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(); 316 383 317 384 #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; 319 387 #endif 320 321 388 } 322 389
Note:
See TracChangeset
for help on using the changeset viewer.