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

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2021/12/22

//  filename:   Plane.cpp

//

//  author:     Armando Alatorre Sevilla, Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    classe definissant un avion

//

/*********************************************************************/



#include "Plane.h"

#include <SimuBldc.h>

#include <SimuServos.h>

#include <TabWidget.h>

#include <Tab.h>

#include <DoubleSpinBox.h>

#include <SpinBox.h>

#include <GroupBox.h>

#include <math.h>

#ifdef GL

#include <ISceneManager.h>

#include <IMeshManipulator.h>

#include "Blade.h"

#include "MeshSceneNode.h"

#include "Gui.h"

#include <Mutex.h>

#endif



#define K_MOT 0.4f    // blade animation

#define G (float)9.81 // gravity ( N/(m/s²) )



#ifdef GL

using namespace irr::video;

using namespace irr::scene;

using namespace irr::core;

#endif

using namespace flair::core;

using namespace flair::gui;

using namespace flair::actuator;



namespace flair {

namespace simulator {



Plane::Plane(std::string name, uint32_t modelId)

    : Model(name,modelId) {

  Tab *setup_tab = new Tab(GetTabWidget(), "model");

  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);

  

  motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);

  

  motor = new SimuBldc(this, name, 1, modelId,0);

  servos = new SimuServos(this, name, 2, modelId,0);

  

  SetIsReady(true);

}



Plane::~Plane() {

  // les objets irrlicht seront automatiquement detruits (moteurs, helices,

  // pales) par parenté

}



#ifdef GL



void Plane::Draw(void) {

  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length

  // parameter

  // note that the frame used is irrlicht one:

  // left handed, North East Up

  const IGeometryCreator *geo;

  geo = getGui()->getSceneManager()->getGeometryCreator();



  // cylinders are aligned with y axis

  IMesh *motor_mesh = geo->createCylinderMesh(7.5, 5, 16);

  ITexture *texture = getGui()->getTexture("metal047.jpg");

  MeshSceneNode *f_motor = new MeshSceneNode(this, motor_mesh, vector3df(150, 0, 20),

                               vector3df(0, 0, -90), texture);

  f_blade = new Blade(this, vector3df(155, 0,20), vector3df(90, 0, 90));

  

  IMesh *body_mesh = geo->createCylinderMesh(20, 150, 32,SColor(0, 255, 255, 255));

  MeshSceneNode *body = new MeshSceneNode(this, body_mesh, vector3df(0, 0, 20),

                             vector3df(0, 0, -90));

  

  IMesh *wing= geo->createCubeMesh (vector3df(35, 100, 2));

  IMesh *aileron= geo->createCubeMesh (vector3df(10, 90, 2));

  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)

  

  //origine de l'aile en son centre

  MeshSceneNode *l_wing = new MeshSceneNode(this, wing, vector3df(75, -20-50, 20),vector3df(0, 0, 0));

  MeshSceneNode *r_wing = new MeshSceneNode(this, wing, vector3df(75, 20+50, 20),vector3df(0, 0, 0));

                      

       

  l_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, -20-100, 20),vector3df(0, 0, 0));

  MeshSceneNode *l_aileron = new MeshSceneNode(l_axe_aileron, aileron, vector3df(-5, 50, 0),vector3df(0, 0, 0),texture);

 

                        

  r_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, 20, 20),vector3df(0, 0, 0));

  MeshSceneNode *r_aileron = new MeshSceneNode(r_axe_aileron, aileron, vector3df(-5, 50,0),vector3df(0, 0, 0),texture);

                               

  actuators_mutex = new Mutex(this);

  motor_speed = 0;

  for (int i = 0; i < 2; i++) servos_pos[i] = 0;

  ExtraDraw();

}



void Plane::AnimateModel(void) {

  actuators_mutex->GetMutex();

  f_blade->SetRotationSpeed(K_MOT *vector3df( 0,motor_speed,0));

  l_axe_aileron->setRotation(vector3df(0,servos_pos[1]*180./3.14,0));

  r_axe_aileron->setRotation(vector3df(0,servos_pos[0]*180./3.14,0));



  actuators_mutex->ReleaseMutex();



  // adapt UAV size

  /*

  if (arm_length->ValueChanged() == true) {

    setScale(arm_length->Value());

  }*/

}



size_t Plane::dbtSize(void) const {

  return 6 * sizeof(float) + 1 * sizeof(float); // 6ddl+1moteur

}



void Plane::WritedbtBuf(char *dbtbuf) {

}



void Plane::ReaddbtBuf(char *dbtbuf) {

}

#endif // GL



// states are computed on fixed frame NED

// x north

// y east

// z down

void Plane::CalcModel(void) {

  float u_roll, u_pitch, u_yaw, u_thrust;

  Time motorTime,servoTime;

  

#ifdef GL

  actuators_mutex->GetMutex();

#endif // GL

  motor->GetSpeeds(&motor_speed,&motorTime);

  if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {

      if(motor_speed!=0) {

        motor_speed=0;

      }

  }

  servos->GetPositions(servos_pos,&servoTime);

  

#ifdef GL

  actuators_mutex->ReleaseMutex();

#endif // GL

  

 

  /*

  ** ===================================================================

  **    u roll: roll torque

  **

  ** ===================================================================

  */

  /*

  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;



  /// 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

  state[0].W.x =

      (dT() / j_roll->Value()) *

          ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +

           u_roll) +

      state[-1].W.x;

*/

 

  /*

  ** ===================================================================

  **   u pitch : pitch torque

  **

  ** ===================================================================

  */

  /*

  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;



  /// 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

  state[0].W.y =

      (dT() / j_pitch->Value()) *

          ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +

           u_pitch) +

      state[-1].W.y;

*/

  

  /*

  ** ===================================================================

  **    u yaw : yaw torque

  **

  ** ===================================================================

  */

  /*

  u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -

                            fr_speed * fr_speed - rl_speed * rl_speed);



  /// 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

  state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;

*/

 

  // compute quaternion from W

  // Quaternion derivative: dQ = 0.5*(Q*Qw)

  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);



  // Quaternion integration

  state[0].Quat = state[-1].Quat + dQ * dT();

  state[0].Quat.Normalize();



  // Calculation of the thrust from the reference speed of motors

  /*

  u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +

                               rl_speed * rl_speed + rr_speed * rr_speed);

  Vector3D<double> vect(0, 0, -u_thrust);

  vect.Rotate(state[0].Quat);

*/

  /*

      ** ===================================================================

      **     x double integrator

      **

      ** ===================================================================

      */

   /*   

  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;

  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();

 */

  /*

  ** ===================================================================

  **     y double integrator

  **

  ** ===================================================================

  */

  /*

  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;

  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();

*/

  /*

  ** ===================================================================

  **     z double integrator

  **

  ** ===================================================================

  */

  /*

  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;

  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();

*/



#ifndef GL

  if (state[0].Pos.z < 0)

    state[0].Pos.z = 0;

#endif

}



} // end namespace simulator

} // end namespace flair