| [10] | 1 | // %flair:license{
|
|---|
| 2 | // This file is part of the Flair framework distributed under the
|
|---|
| 3 | // CECILL-C License, Version 1.0.
|
|---|
| 4 | // %flair:license}
|
|---|
| [8] | 5 | // created: 2014/04/03
|
|---|
| 6 | // filename: X8.cpp
|
|---|
| 7 | //
|
|---|
| 8 | // author: Majd Saied, Guillaume Sanahuja
|
|---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
|---|
| 10 | //
|
|---|
| 11 | // version: $Id: $
|
|---|
| 12 | //
|
|---|
| 13 | // purpose: classe definissant un X8
|
|---|
| 14 | //
|
|---|
| 15 | /*********************************************************************/
|
|---|
| 16 |
|
|---|
| 17 | #include "X8.h"
|
|---|
| 18 | #include "Simulator.h"
|
|---|
| 19 | #include <SimuBldc.h>
|
|---|
| 20 | #include <TabWidget.h>
|
|---|
| 21 | #include <Tab.h>
|
|---|
| 22 | #include <DoubleSpinBox.h>
|
|---|
| 23 | #include <GroupBox.h>
|
|---|
| 24 | #include <math.h>
|
|---|
| 25 | #ifdef GL
|
|---|
| 26 | #include <ISceneManager.h>
|
|---|
| 27 | #include "Blade.h"
|
|---|
| 28 | #include "MeshSceneNode.h"
|
|---|
| 29 | #include "Gui.h"
|
|---|
| 30 | #include <Mutex.h>
|
|---|
| 31 | #endif
|
|---|
| 32 |
|
|---|
| 33 | #define K_MOT 0.4f //blade animation
|
|---|
| 34 | #define G (float)9.81 //gravity ( N/(m/s²) )
|
|---|
| 35 |
|
|---|
| 36 | #ifdef GL
|
|---|
| 37 | using namespace irr::video;
|
|---|
| 38 | using namespace irr::scene;
|
|---|
| 39 | using namespace irr::core;
|
|---|
| 40 | #endif
|
|---|
| 41 | using namespace flair::core;
|
|---|
| 42 | using namespace flair::gui;
|
|---|
| 43 | using namespace flair::actuator;
|
|---|
| 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();
|
|---|
| 316 |
|
|---|
| 317 | #ifndef GL
|
|---|
| 318 | if(state[0].Pos.z<0) state[0].Pos.z=0;
|
|---|
| 319 | #endif
|
|---|
| 320 |
|
|---|
| 321 | }
|
|---|
| 322 |
|
|---|
| 323 | } // end namespace simulator
|
|---|
| 324 | } // end namespace flair
|
|---|
