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

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

modifs 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 f_blade->SetRotationSpeed(K_MOT *vector3df( 0,motor_speed,0));
114 l_axe_aileron->setRotation(vector3df(0,servos_pos[1]*180./3.14,0));
115 r_axe_aileron->setRotation(vector3df(0,servos_pos[0]*180./3.14,0));
116
117 actuators_mutex->ReleaseMutex();
118
119 // adapt UAV size
120 /*
121 if (arm_length->ValueChanged() == true) {
122 setScale(arm_length->Value());
123 }*/
124}
125
126size_t Plane::dbtSize(void) const {
127 return 6 * sizeof(float) + 1 * sizeof(float); // 6ddl+1moteur
128}
129
130void Plane::WritedbtBuf(char *dbtbuf) {
131}
132
133void Plane::ReaddbtBuf(char *dbtbuf) {
134}
135#endif // GL
136
137// states are computed on fixed frame NED
138// x north
139// y east
140// z down
141void Plane::CalcModel(void) {
142 float u_roll, u_pitch, u_yaw, u_thrust;
143 Time motorTime,servoTime;
144
145#ifdef GL
146 actuators_mutex->GetMutex();
147#endif // GL
148 motor->GetSpeeds(&motor_speed,&motorTime);
149 if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
150 if(motor_speed!=0) {
151 motor_speed=0;
152 }
153 }
154 servos->GetPositions(servos_pos,&servoTime);
155
156#ifdef GL
157 actuators_mutex->ReleaseMutex();
158#endif // GL
159
160
161 /*
162 ** ===================================================================
163 ** u roll: roll torque
164 **
165 ** ===================================================================
166 */
167 /*
168 u_roll = arm_length->Value() * k_mot->Value() *
169 (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
170 rr_speed * rr_speed) *
171 sqrtf(2) / 2;
172
173 /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
174 /// of the quadri in the body frame). It is a discrete integrator
175 state[0].W.x =
176 (dT() / j_roll->Value()) *
177 ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
178 u_roll) +
179 state[-1].W.x;
180*/
181
182 /*
183 ** ===================================================================
184 ** u pitch : pitch torque
185 **
186 ** ===================================================================
187 */
188 /*
189 u_pitch = arm_length->Value() * k_mot->Value() *
190 (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
191 rr_speed * rr_speed) *
192 sqrtf(2) / 2;
193
194 /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
195 /// of the quadri in the body frame). It is a discrete integrator
196 state[0].W.y =
197 (dT() / j_pitch->Value()) *
198 ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
199 u_pitch) +
200 state[-1].W.y;
201*/
202
203 /*
204 ** ===================================================================
205 ** u yaw : yaw torque
206 **
207 ** ===================================================================
208 */
209 /*
210 u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
211 fr_speed * fr_speed - rl_speed * rl_speed);
212
213 /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
214 /// of the quadri in the body frame). It is a discrete integrator
215 state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
216*/
217
218 // compute quaternion from W
219 // Quaternion derivative: dQ = 0.5*(Q*Qw)
220 Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
221
222 // Quaternion integration
223 state[0].Quat = state[-1].Quat + dQ * dT();
224 state[0].Quat.Normalize();
225
226 // Calculation of the thrust from the reference speed of motors
227 /*
228 u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
229 rl_speed * rl_speed + rr_speed * rr_speed);
230 Vector3D<double> vect(0, 0, -u_thrust);
231 vect.Rotate(state[0].Quat);
232*/
233 /*
234 ** ===================================================================
235 ** x double integrator
236 **
237 ** ===================================================================
238 */
239 /*
240 state[0].Pos.x =
241 (dT() * dT() / m->Value()) *
242 (vect.x - f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
243 2 * state[-1].Pos.x - state[-2].Pos.x;
244 state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
245 */
246 /*
247 ** ===================================================================
248 ** y double integrator
249 **
250 ** ===================================================================
251 */
252 /*
253 state[0].Pos.y =
254 (dT() * dT() / m->Value()) *
255 (vect.y -
256 f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
257 2 * state[-1].Pos.y - state[-2].Pos.y;
258 state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
259*/
260 /*
261 ** ===================================================================
262 ** z double integrator
263 **
264 ** ===================================================================
265 */
266 /*
267 state[0].Pos.z =
268 (dT() * dT() / m->Value()) *
269 (vect.z +
270 f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
271 m->Value() * G) +
272 2 * state[-1].Pos.z - state[-2].Pos.z;
273 state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
274*/
275
276#ifndef GL
277 if (state[0].Pos.z < 0)
278 state[0].Pos.z = 0;
279#endif
280}
281
282} // end namespace simulator
283} // end namespace flair
Note: See TracBrowser for help on using the repository browser.