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

Last change on this file since 458 was 458, checked in by Sanahuja Guillaume, 7 months ago

update plane

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