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

Last change on this file since 370 was 370, checked in by Sanahuja Guillaume, 13 months ago

add abilitiy to change x4 and x8 arm color from ground station

File size: 13.5 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[8]5//  created:    2012/08/21
6//  filename:   X4.cpp
7//
8//  author:     Osamah Saif, Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    classe definissant un x4
14//
15/*********************************************************************/
16
17#include "X4.h"
18#include <SimuBldc.h>
19#include <TabWidget.h>
20#include <Tab.h>
21#include <DoubleSpinBox.h>
[214]22#include <SpinBox.h>
[8]23#include <GroupBox.h>
24#include <math.h>
25#ifdef GL
26#include <ISceneManager.h>
[370]27#include <IMeshManipulator.h>
[8]28#include "Blade.h"
29#include "MeshSceneNode.h"
30#include "Gui.h"
31#include <Mutex.h>
32#endif
33
[15]34#define K_MOT 0.4f    // blade animation
35#define G (float)9.81 // gravity ( N/(m/s²) )
[8]36
37#ifdef GL
38using namespace irr::video;
39using namespace irr::scene;
40using namespace irr::core;
41#endif
42using namespace flair::core;
43using namespace flair::gui;
44using namespace flair::actuator;
45
[15]46namespace flair {
47namespace simulator {
[8]48
[158]49X4::X4(std::string name, uint32_t modelId)
50    : Model(name,modelId) {
[15]51  Tab *setup_tab = new Tab(GetTabWidget(), "model");
52  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
53  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
54                                 0, 2, 0.1);
55  // l_cg=new DoubleSpinBox(setup_tab,"position G
56  // (m):",0,2,-0.5,0.5,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)
[214]77                           
78  motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);
[370]79 
80  Tab *visual_tab = new Tab(GetTabWidget(), "visual");
81  armColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
82  armColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
83  armColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
[8]84
[158]85  motors = new SimuBldc(this, name, 4, modelId,0);
[157]86 
87  SetIsReady(true);
[8]88}
89
[15]90X4::~X4() {
91  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
92  // pales) par parenté
[8]93}
94
95#ifdef GL
96
[15]97void X4::Draw(void) {
98  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
99  // parameter
100  // note that the frame used is irrlicht one:
101  // left handed, North East Up
102  const IGeometryCreator *geo;
103  geo = getGui()->getSceneManager()->getGeometryCreator();
[8]104
[15]105  // cylinders are aligned with y axis
[370]106  colored_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, armColorR->Value(), armColorG->Value(), armColorB->Value()));
[158]107  IMesh *black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
108  IMesh *motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
[15]109  // geo->drop();
[8]110
[15]111  ITexture *texture = getGui()->getTexture("carbone.jpg");
[370]112  MeshSceneNode *fl_arm = new MeshSceneNode(this, colored_arm, vector3df(0, 0, 0),
[15]113                             vector3df(0, 0, -135));
[370]114  MeshSceneNode *fr_arm = new MeshSceneNode(this, colored_arm, vector3df(0, 0, 0),
[15]115                             vector3df(0, 0, -45));
[158]116  MeshSceneNode *rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
[15]117                             vector3df(0, 0, 135), texture);
[158]118  MeshSceneNode *rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
[15]119                             vector3df(0, 0, 45), texture);
[8]120
[15]121  texture = getGui()->getTexture("metal047.jpg");
[158]122  MeshSceneNode *fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
[15]123                               vector3df(90, 0, 0), texture);
[158]124  MeshSceneNode *fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
[15]125                               vector3df(90, 0, 0), texture);
[158]126  MeshSceneNode *rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
[15]127                               vector3df(90, 0, 0), texture);
[158]128  MeshSceneNode *rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
[15]129                               vector3df(90, 0, 0), texture);
[8]130
[15]131  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
[339]132  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), vector3df(0, 0, 0),true);
133  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5),  vector3df(0, 0, 0),true);
[15]134  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
[8]135
[15]136  motor_speed_mutex = new Mutex(this);
137  for (int i = 0; i < 4; i++)
138    motor_speed[i] = 0;
139  ExtraDraw();
[8]140}
141
[15]142void X4::AnimateModel(void) {
143  motor_speed_mutex->GetMutex();
[339]144  fl_blade->SetRotationSpeed(K_MOT *vector3df(0, 0, motor_speed[0]));
145  fr_blade->SetRotationSpeed(-K_MOT *vector3df(0, 0, motor_speed[1]));
146  rl_blade->SetRotationSpeed(-K_MOT *vector3df(0, 0, motor_speed[2]));
147  rr_blade->SetRotationSpeed(K_MOT *vector3df(0, 0, motor_speed[3]));
[15]148  motor_speed_mutex->ReleaseMutex();
[370]149 
150  if (armColorR->ValueChanged() == true || armColorG->ValueChanged() == true || armColorB->ValueChanged() == true) {
151    getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_arm, SColor(0,armColorR->Value(), armColorG->Value(), armColorB->Value()));
152  }
[8]153
[15]154  // adapt UAV size
155  if (arm_length->ValueChanged() == true) {
156    setScale(arm_length->Value());
157  }
[8]158}
159
[15]160size_t X4::dbtSize(void) const {
161  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
[8]162}
163
[15]164void X4::WritedbtBuf(
165    char *dbtbuf) { /*
166                       float *buf=(float*)dbtbuf;
167                       vector3df vect=getPosition();
168                       memcpy(buf,&vect.X,sizeof(float));
169                       buf++;
170                       memcpy(buf,&vect.Y,sizeof(float));
171                       buf++;
172                       memcpy(buf,&vect.Z,sizeof(float));
173                       buf++;
174                       vect=getRotation();
175                       memcpy(buf,&vect.X,sizeof(float));
176                       buf++;
177                       memcpy(buf,&vect.Y,sizeof(float));
178                       buf++;
179                       memcpy(buf,&vect.Z,sizeof(float));
180                       buf++;
181                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
[8]182}
183
[15]184void X4::ReaddbtBuf(
185    char *dbtbuf) { /*
186                       float *buf=(float*)dbtbuf;
187                       vector3df vect;
188                       memcpy(&vect.X,buf,sizeof(float));
189                       buf++;
190                       memcpy(&vect.Y,buf,sizeof(float));
191                       buf++;
192                       memcpy(&vect.Z,buf,sizeof(float));
193                       buf++;
194                       setPosition(vect);
195                       memcpy(&vect.X,buf,sizeof(float));
196                       buf++;
197                       memcpy(&vect.Y,buf,sizeof(float));
198                       buf++;
199                       memcpy(&vect.Z,buf,sizeof(float));
200                       buf++;
201                       ((ISceneNode*)(this))->setRotation(vect);
202                       memcpy(&motors,buf,sizeof(rtsimu_motors));
203                       AnimateModele();*/
[8]204}
[15]205#endif // GL
[8]206
[15]207// states are computed on fixed frame NED
208// x north
209// y east
210// z down
211void X4::CalcModel(void) {
212  float fl_speed, fr_speed, rl_speed, rr_speed;
213  float u_roll, u_pitch, u_yaw, u_thrust;
[214]214  Time motorTime;
[8]215#ifdef GL
[15]216  motor_speed_mutex->GetMutex();
217#endif // GL
[214]218  motors->GetSpeeds(motor_speed,&motorTime);
219  if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
220    for(int i=0;i<4;i++) {
221      if(motor_speed[i]!=0) {
222         //Printf("timout\n");
223        for(int i=0;i<4;i++) motor_speed[i]=0;
224        break;
225      }
226    }
227  }
[8]228#ifdef GL
[15]229  motor_speed_mutex->ReleaseMutex();
230#endif // GL
[214]231 
[15]232  fl_speed = motor_speed[0];
233  fr_speed = motor_speed[1];
234  rl_speed = motor_speed[2];
235  rr_speed = motor_speed[3];
[8]236
[15]237  /*
[214]238  ** ===================================================================
239  **    u roll: roll torque
240  **
241  ** ===================================================================
242  */
[15]243  u_roll = arm_length->Value() * k_mot->Value() *
244           (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
245            rr_speed * rr_speed) *
246           sqrtf(2) / 2;
[8]247
[15]248  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
249  /// of the quadri in the body frame). It is a discrete integrator
250  state[0].W.x =
251      (dT() / j_roll->Value()) *
252          ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
253           u_roll) +
254      state[-1].W.x;
[8]255
[15]256  // 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;
257  // 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;
[8]258
[15]259  /*
260  ** ===================================================================
261  **   u pitch : pitch torque
262  **
263  ** ===================================================================
264  */
265  u_pitch = arm_length->Value() * k_mot->Value() *
266            (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
267             rr_speed * rr_speed) *
268            sqrtf(2) / 2;
[8]269
[15]270  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
271  /// of the quadri in the body frame). It is a discrete integrator
272  state[0].W.y =
273      (dT() / j_pitch->Value()) *
274          ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
275           u_pitch) +
276      state[-1].W.y;
[8]277
[15]278  // 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;
279  // 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;
[8]280
[15]281  /*
282  ** ===================================================================
283  **    u yaw : yaw torque
284  **
285  ** ===================================================================
286  */
287  u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
288                            fr_speed * fr_speed - rl_speed * rl_speed);
[8]289
[15]290  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
291  /// of the quadri in the body frame). It is a discrete integrator
292  state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
[8]293
[15]294  // u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
295  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
[8]296
[15]297  // compute quaternion from W
298  // Quaternion derivative: dQ = 0.5*(Q*Qw)
299  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
[8]300
[15]301  // Quaternion integration
302  state[0].Quat = state[-1].Quat + dQ * dT();
303  state[0].Quat.Normalize();
[8]304
[15]305  // Calculation of the thrust from the reference speed of motors
306  u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
307                               rl_speed * rl_speed + rr_speed * rr_speed);
[167]308  Vector3D<double> vect(0, 0, -u_thrust);
[15]309  vect.Rotate(state[0].Quat);
[8]310
[15]311  /*
312      ** ===================================================================
313      **     x double integrator
314      **
315      ** ===================================================================
316      */
[167]317     
[15]318  state[0].Pos.x =
319      (dT() * dT() / m->Value()) *
[167]320          (vect.x - f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
[15]321      2 * state[-1].Pos.x - state[-2].Pos.x;
322  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
[167]323 
[15]324  /*
325  ** ===================================================================
326  **     y double integrator
327  **
328  ** ===================================================================
329  */
330  state[0].Pos.y =
331      (dT() * dT() / m->Value()) *
332          (vect.y -
333           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
334      2 * state[-1].Pos.y - state[-2].Pos.y;
335  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
[8]336
[15]337  /*
338  ** ===================================================================
339  **     z double integrator
340  **
341  ** ===================================================================
342  */
343  state[0].Pos.z =
344      (dT() * dT() / m->Value()) *
345          (vect.z +
346           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
347           m->Value() * G) +
348      2 * state[-1].Pos.z - state[-2].Pos.z;
349  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
[8]350
351#ifndef GL
[15]352  if (state[0].Pos.z < 0)
353    state[0].Pos.z = 0;
[8]354#endif
355}
356
357} // end namespace simulator
358} // end namespace flair
Note: See TracBrowser for help on using the repository browser.