source: flair-src/trunk/lib/FlairSimulator/src/Plane.cpp@ 459

Last change on this file since 459 was 459, checked in by Sanahuja Guillaume, 6 months ago

modif plane

File size: 8.7 KB
Line 
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}
5// created: 2021/12/22
6// filename: Plane.cpp
7//
8// author: Armando Alatorre Sevilla, Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: classe definissant un avion
14//
15/*********************************************************************/
16
17#include "Plane.h"
18#include <SimuBldc.h>
19#include <SimuServos.h>
20#include <TabWidget.h>
21#include <Tab.h>
22#include <DoubleSpinBox.h>
23#include <SpinBox.h>
24#include <GroupBox.h>
25#include <math.h>
26#ifdef GL
27#include <ISceneManager.h>
28#include <IMeshManipulator.h>
29#include "Blade.h"
30#include "MeshSceneNode.h"
31#include "Gui.h"
32#include <Mutex.h>
33#endif
34
35#define K_MOT 0.4f // blade animation
36#define G (float)9.81 // gravity ( N/(m/s²) )
37
38#ifdef GL
39using namespace irr::video;
40using namespace irr::scene;
41using namespace irr::core;
42#endif
43using namespace flair::core;
44using namespace flair::gui;
45using namespace flair::actuator;
46
47namespace flair {
48namespace simulator {
49
50Plane::Plane(std::string name, uint32_t modelId)
51 : Model(name,modelId) {
52 Tab *setup_tab = new Tab(GetTabWidget(), "model");
53 m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
54
55 motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);
56
57 motor = new SimuBldc(this, name, 1, modelId,0);
58 servos = new SimuServos(this, name, 2, modelId,0);
59
60 SetIsReady(true);
61}
62
63Plane::~Plane() {
64 // les objets irrlicht seront automatiquement detruits (moteurs, helices,
65 // pales) par parenté
66}
67
68#ifdef GL
69
70void Plane::Draw(void) {
71 // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
72 // parameter
73 // note that the frame used is irrlicht one:
74 // left handed, North East Up
75 const IGeometryCreator *geo;
76 geo = getGui()->getSceneManager()->getGeometryCreator();
77
78 // cylinders are aligned with y axis
79 IMesh *motor_mesh = geo->createCylinderMesh(7.5, 5, 16);
80 ITexture *texture = getGui()->getTexture("metal047.jpg");
81 MeshSceneNode *f_motor = new MeshSceneNode(this, motor_mesh, vector3df(150, 0, 20),
82 vector3df(0, 0, -90), texture);
83 f_blade = new Blade(this, vector3df(155, 0,20), vector3df(90, 0, 90));
84
85 IMesh *body_mesh = geo->createCylinderMesh(20, 150, 32,SColor(0, 255, 255, 255));
86 MeshSceneNode *body = new MeshSceneNode(this, body_mesh, vector3df(0, 0, 20),
87 vector3df(0, 0, -90));
88
89 IMesh *wing= geo->createCubeMesh (vector3df(35, 100, 2));
90 IMesh *aileron= geo->createCubeMesh (vector3df(10, 90, 2));
91 IMesh *axe = geo->createCylinderMesh(.5, 100, 16);//axe pour definir l'axe de rotation en bout d'aileron (sinon l'axe est au milieu d'aileron)
92
93 //origine de l'aile en son centre
94 MeshSceneNode *l_wing = new MeshSceneNode(this, wing, vector3df(75, -20-50, 20),vector3df(0, 0, 0));
95 MeshSceneNode *r_wing = new MeshSceneNode(this, wing, vector3df(75, 20+50, 20),vector3df(0, 0, 0));
96
97
98 l_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, -20-100, 20),vector3df(0, 0, 0));
99 MeshSceneNode *l_aileron = new MeshSceneNode(l_axe_aileron, aileron, vector3df(-5, 50, 0),vector3df(0, 0, 0),texture);
100
101
102 r_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, 20, 20),vector3df(0, 0, 0));
103 MeshSceneNode *r_aileron = new MeshSceneNode(r_axe_aileron, aileron, vector3df(-5, 50,0),vector3df(0, 0, 0),texture);
104
105 actuators_mutex = new Mutex(this);
106 motor_speed = 0;
107 for (int i = 0; i < 2; i++) servos_pos[i] = 0;
108 ExtraDraw();
109}
110
111void Plane::AnimateModel(void) {
112 actuators_mutex->GetMutex();
113 motor_speed=10;
114 f_blade->SetRotationSpeed(K_MOT *vector3df( 0,motor_speed,0));
115 l_axe_aileron->setRotation(vector3df(0,servos_pos[1]*180./3.14,0));
116 r_axe_aileron->setRotation(vector3df(0,servos_pos[0]*180./3.14,0));
117
118 actuators_mutex->ReleaseMutex();
119
120 // adapt UAV size
121 /*
122 if (arm_length->ValueChanged() == true) {
123 setScale(arm_length->Value());
124 }*/
125}
126
127size_t Plane::dbtSize(void) const {
128 return 6 * sizeof(float) + 1 * sizeof(float); // 6ddl+1moteur
129}
130
131void Plane::WritedbtBuf(char *dbtbuf) {
132}
133
134void Plane::ReaddbtBuf(char *dbtbuf) {
135}
136#endif // GL
137
138// states are computed on fixed frame NED
139// x north
140// y east
141// z down
142void Plane::CalcModel(void) {
143 float u_roll, u_pitch, u_yaw, u_thrust;
144 Time motorTime,servoTime;
145
146#ifdef GL
147 actuators_mutex->GetMutex();
148#endif // GL
149 motor->GetSpeeds(&motor_speed,&motorTime);
150 if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
151 if(motor_speed!=0) {
152 motor_speed=0;
153 }
154 }
155 servos->GetPositions(servos_pos,&servoTime);
156
157#ifdef GL
158 actuators_mutex->ReleaseMutex();
159#endif // GL
160
161
162 /*
163 ** ===================================================================
164 ** u roll: roll torque
165 **
166 ** ===================================================================
167 */
168 /*
169 u_roll = arm_length->Value() * k_mot->Value() *
170 (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
171 rr_speed * rr_speed) *
172 sqrtf(2) / 2;
173
174 /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
175 /// of the quadri in the body frame). It is a discrete integrator
176 state[0].W.x =
177 (dT() / j_roll->Value()) *
178 ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
179 u_roll) +
180 state[-1].W.x;
181*/
182
183 /*
184 ** ===================================================================
185 ** u pitch : pitch torque
186 **
187 ** ===================================================================
188 */
189 /*
190 u_pitch = arm_length->Value() * k_mot->Value() *
191 (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
192 rr_speed * rr_speed) *
193 sqrtf(2) / 2;
194
195 /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
196 /// of the quadri in the body frame). It is a discrete integrator
197 state[0].W.y =
198 (dT() / j_pitch->Value()) *
199 ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
200 u_pitch) +
201 state[-1].W.y;
202*/
203
204 /*
205 ** ===================================================================
206 ** u yaw : yaw torque
207 **
208 ** ===================================================================
209 */
210 /*
211 u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
212 fr_speed * fr_speed - rl_speed * rl_speed);
213
214 /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
215 /// of the quadri in the body frame). It is a discrete integrator
216 state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
217*/
218
219 // compute quaternion from W
220 // Quaternion derivative: dQ = 0.5*(Q*Qw)
221 Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
222
223 // Quaternion integration
224 state[0].Quat = state[-1].Quat + dQ * dT();
225 state[0].Quat.Normalize();
226
227 // Calculation of the thrust from the reference speed of motors
228 /*
229 u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
230 rl_speed * rl_speed + rr_speed * rr_speed);
231 Vector3D<double> vect(0, 0, -u_thrust);
232 vect.Rotate(state[0].Quat);
233*/
234 /*
235 ** ===================================================================
236 ** x double integrator
237 **
238 ** ===================================================================
239 */
240 /*
241 state[0].Pos.x =
242 (dT() * dT() / m->Value()) *
243 (vect.x - f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
244 2 * state[-1].Pos.x - state[-2].Pos.x;
245 state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
246 */
247 /*
248 ** ===================================================================
249 ** y double integrator
250 **
251 ** ===================================================================
252 */
253 /*
254 state[0].Pos.y =
255 (dT() * dT() / m->Value()) *
256 (vect.y -
257 f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
258 2 * state[-1].Pos.y - state[-2].Pos.y;
259 state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
260*/
261 /*
262 ** ===================================================================
263 ** z double integrator
264 **
265 ** ===================================================================
266 */
267 /*
268 state[0].Pos.z =
269 (dT() * dT() / m->Value()) *
270 (vect.z +
271 f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
272 m->Value() * G) +
273 2 * state[-1].Pos.z - state[-2].Pos.z;
274 state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
275*/
276
277#ifndef GL
278 if (state[0].Pos.z < 0)
279 state[0].Pos.z = 0;
280#endif
281}
282
283} // end namespace simulator
284} // end namespace flair
Note: See TracBrowser for help on using the repository browser.