Implémentation du model physique avec Raydium et ODE
RetourApres avoir déterminé les équations physiques régissant le comportement du simulateur, nous allons détailler le code et les différentes fonctions de Raydium que nous avons utilisées.
La création d’une scène basique étant détaillée dans les tutoriaux présents sur le site de Raydium, nous allons seulement nous attacher à décrire les modifications que nous y avons apportées ainsi que les nouvelles fonctions créées.
Les fonctions liées à la communication série sont explicitées dans la partie SimulCapteurs.
Les fonctions sont les suivantes :
- Définition des constantes
- Définition des variables globales
- Simul_create
- Camera
- Vehicule_commande
- Clavier
- Chrono
- Display
- Periph_write
- Periph_read
- Step
- Simul_init
- Calculs
- Main
Définition des constantes :
Un certain nombre de constantes ont été déclarées, celles-ci correspondent aux caractéristiques réelles du drone (taille, poids, coefficients de portance …etc..). Ainsi, ces valeurs peuvent être modifiées facilement au cas où l’on voudrait modifier les caractéristiques du drone.
Définition des variables globales :
Les variables contenant les portances à appliquer ainsi que les différents angles sont définis en global pour pouvoir être utilisés dans différentes fonctions.
float portanceZaile1=0; //portance appliquee en Z a la demi-aile droite float portanceZaile2=0; //portance appliquee en Z a la demi-aile gauche float portanceZemp=0; //portance appliquee en Z a l'empenage float portanceYemp=0; //portance appliquee en Y a l'empenage float portanceYcorps=0; //portance appliquee en Y au corps float alpha0aile1=0; //volets aile droite float alpha0aile2=0; //volet aile gauche float alpha0emp=0; //volet empenage monter/descendre float alpha0emp1=0; //volet empenage gauche/droite float alphaz=0; //angle entre le vecteur vitesse et l'angle // d'inclinaison du drone en Z float alphay=0; //angle entre le vecteur vitesse et l'angle // d'inclinaison du drone en Y float traine=0; //force de traine (oppose a la traction) float traction; //force du moteur
On y trouve aussi par exemple le nom du décor dans lequel évolue le drone.
char *piste="mapiut4.tri";
La variable mode qui permet de sélectionner soit le mode de commande manuelle soit le mode de commande asservie par le microcontrôleur via le port série.
char MODE = 0 ; // mode 0 = commande clavier, 1 = liaison serie
Les variables permettant à ODE d’identifier les objets auquels appliquer les forces:
dBodyID bodycorp; //variable liee au corps dBodyID bodyaile1; //variable liee a l'aile droite dBodyID bodyaile2; //variable liee a l'aile gauche dBodyID bodyemp; //variable liee a l'empenage dBodyID bodytraction; //variable liee a la traction
Les autres variables seront décrites au moment de leurs utilisations.
Simul_create :
Cette fonction permet de créer les éléments composant le model physique du drone. Ces éléments sont contenus dans un objet.
objet_drone=raydium_ode_object_create("drone");
Les éléments composant le drone sont en fait des boites, sur lesquelles vont s’appliquer les forces.
Un premier élément est créé, représentant le corps du drone auquel on associera le rendu créé sous Blender (CreatMap).
Ces éléments sont créés avec les données définies au début du programme.
//Creation du corps du drone avec ses donnees physiques reelless, auquel on //associe le rendu contenu dans un fichier .tri raydium_ode_object_box_add("corps",objet_drone,POID_CORPS,(LONGUEUR_CORPS +LONGUEUR_EMPENNAGE)*1,(EPAISSEUR_COPRS)*1,LARGEUR_CORPS*1,RAYDIUM_ODE_STANDARD, TYPE_DRONE,"drone.tri"); //definition des frottements associes au corps raydium_ode_element_slip_name("corps",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); //definition des cotnraintes associees au corps raydium_ode_element_material_name("corps",gerp,gcfm);
Les autres éléments composant le drone sont les deux ailes, l’empennage, un objet représentant le moteur et l’hélice ainsi qu’une batterie placée à 30% du bord d'attaque des ailes pour que le comportement du drone soit similaire à celui d'un modèle réduit.
//Creation de l'aile droite du drone avec ses donnees physiques reelless raydium_ode_object_box_add("aile1",objet_drone,POID_DEMI_AILE,LONGUEUR_AILE, LARGEUR_DEMI_AILE,EPAISSEUR_AILE,RAYDIUM_ODE_STANDARD,TYPE_DRONE,""); raydium_ode_element_slip_name("aile1",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("aile1",gerp,gcfm); //Creation de l'aile gauche du drone avec ses donnees physiques reelles raydium_ode_object_box_add("aile2",objet_drone,POID_DEMI_AILE,LONGUEUR_AILE, LARGEUR_DEMI_AILE,EPAISSEUR_AILE,RAYDIUM_ODE_STANDARD,TYPE_DRONE,""); raydium_ode_element_slip_name("aile2",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("aile2",gerp,gcfm); //Creation de l'empennage du drone avec ses donnees physiques reelles raydium_ode_object_box_add("empennage",objet_drone,POID_EMPENNAGE, LONGUEUR_EMPENNAGE,LARGEUR_EMPENNAGE,EPAISSEUR_EMPENNAGE,RAYDIUM_ODE_STANDARD, TYPE_DRONE,""); raydium_ode_element_slip_name("empennage",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("empennage",gerp,gcfm); // Creation de l'objet representant l'emplacement du moteur et de l'helice // du drone avec son poids raydium_ode_object_box_add("traction",objet_drone,POID_MOTEUR,0.01,0.01,0.01,//boite d'1 centimetre de cote RAYDIUM_ODE_STANDARD,TYPE_DRONE,""); raydium_ode_element_slip_name("traction",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("traction",gerp,gcfm); // Creation de l'objet permettant de deplacer le centre d'inertie // du drone, cet objet sera deplacé en fonction des differentes données // physique du drone definie // Le balancier est la representation de la batterie raydium_ode_object_box_add("balancier",objet_drone,0.05,0.1,0.1,0.1, RAYDIUM_ODE_STANDARD,TYPE_DRONE,""); raydium_ode_element_slip_name("balancier",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("balancier",gerp,gcfm);
Les objets sont créés tous au même endroit, il faut donc les placer au bon endroit afin d’obtenir un assemblage correspondant à la forme du drone. Ces placements sont faits automatiquement en fonction des tailles de chaque objet.
L'emplacement de la batterie est déterminé mathématiquement d'après les données physiques du drone et influence, de par son poid élevé, le centre de gravité du drone.
//Placement des ailes par rapport au drone float decalageaile = (EPAISSEUR_COPRS/2)+(LARGEUR_DEMI_AILE/2); raydium_ode_element_move_name_3f("aile1",(LONGUEUR_EMPENAGE/2),decalageaile,0); raydium_ode_element_move_name_3f("aile2",(LONGUEUR_EMPENAGE/2),-decalageaile,0); //Placement de l'empenage par rapport au drone float decalageempenage = (LONGUEUR_CORPS/2)-0.01; raydium_ode_element_move_name_3f("empenage",-decalageempenage,0,0); //Placement de la traction par rapport au drone float decalagetraction = (LONGUEUR_CORPS/2)+(0.1/2)+(LONGUEUR_EMPENAGE/2); raydium_ode_element_move_name_3f("traction",decalagetraction,0,0); //Placement du balancier par rapport au drone raydium_ode_element_move_name_3f("balancier",0.16,0,-0.1);
Les objets étant placés aux bons endroits, il faut les fixer entre eux pour que l’ensemble reste solidaire au moment des déplacements.
Pour éviter des problèmes de stabilité, nous avons fixé tous les éléments à un seul élément, à savoir le corps du drone.
//Creation des joints pour lier tout les element ensemble raydium_ode_joint_attach_fixed_name("joint","corps","aile1"); raydium_ode_joint_attach_fixed_name("joint2","corps","aile2"); raydium_ode_joint_attach_fixed_name("joint3","corps","empenage"); raydium_ode_joint_attach_fixed_name("joint4","corps","traction"); raydium_ode_joint_attach_fixed_name("joint5","corps","balancier");
D’après le cahier des charges, le drone doit décoller verticalement, or, par défaut celui-ci est à plat, il faut donc le tourner de 90° selon l’axe des Y pour le placer dans la bonne position. Nous l’avons aussi légèrement déplacé en hauteur pour ne pas qu’il apparaisse incrusté dans le sol au moment de la rotation.
//Place le dronne verticalement, pret pour le decolage !!!! //translation du drone x y z, z etant la hauteur raydium_ode_object_move_name_3f("drone",0,400,0.7); raydium_ode_object_rotate_name_3f("drone",PI/2,0,PI/2);
Nous devons aussi récupérer l’identifiant de chaque objet créé précédemment pour permettre d’appliquer les forces sur chacun de ces éléments dans la suite du programme.
Les fonctions permettant de réaliser ceci sont des fonctions propres à ODE qui ne sont donc pas présentes dans la liste des commandes de raydium mais sur celle d’ODE.
/*---------------------------------------------------------------------------*/ /* recuperation des Id de chaque elements */ bcorps = raydium_ode_element_find("corps"); bodycorp = raydium_ode_element[bcorps].body; baile1 = raydium_ode_element_find("aile1"); bodyaile1 = raydium_ode_element[baile1].body; baile2 = raydium_ode_element_find("aile2"); bodyaile2 = raydium_ode_element[baile2].body; bemp = raydium_ode_element_find("empenage"); bodyemp = raydium_ode_element[bemp].body; btrac = raydium_ode_element_find("traction"); bodytraction = raydium_ode_element[btrac].body; /*---------------------------------------------------------------------------*/
Caméra :
Cette fonction a pour rôle de sélectionner le mode de caméra souhaité, libre ou suivi.
Le mode libre permet de déplacer la caméra à travers la carte grâce au bouton de la souris
if (vehicule==-1 || mode_camera==libre) { int delta_x, delta_y; float move=0; //mouvement de la souris en cas de clique if (raydium_mouse_button[1]) move=-5; if (raydium_mouse_button[0]) move=5; if (raydium_mouse_button[2]) move=0.1; if (raydium_mouse_click==4) move=0.01; if (raydium_mouse_click==5) move=-0.01; cam_pos_z += (raydium_trigo_sin(cam_angle_x+90)*move*speed* raydium_trigo_sin(90-cam_angle_y)); cam_pos_x += (raydium_trigo_cos(cam_angle_x+90)*move*speed* raydium_trigo_sin(90-cam_angle_y)); cam_pos_y += (raydium_trigo_cos(90-cam_angle_y)*speed*move); delta_x = raydium_mouse_x - (raydium_window_tx/2); cam_angle_x += (delta_x*sensibilite); delta_y = raydium_mouse_y - (raydium_window_ty/2); cam_angle_y += (delta_y*sensibilite); raydium_mouse_move(raydium_window_tx/2, raydium_window_ty/2); raydium_camera_place(cam_pos_x,cam_pos_y,cam_pos_z,cam_angle_x, cam_angle_y,0); return; }
Le mode suivi permet de suivre le drone à une distance prédéfinie tout au long de ces déplacements. Pour se faire, cette fonction récupère la position du drone ainsi que la vitesse celui-ci, ce qui lui permet d'en déduire où se trouve le drone, comment il se déplace et donc de le suivre.
if (mode_camera==suivi) { dReal *vel; dReal *pos; dReal cam[3]; dReal mpos[3]; // get element position pos=raydium_ode_element_pos_get_name("corps"); vel=raydium_ode_element_linearvelocity_get_name("corps"); // get position of camera in world coords raydium_ode_element_RelPointPos_name("corps",-3,0,1,cam); // correct z pos (always at top of the car, for example) cam[2]=pos[2]+0.6; cam[0]=pos[0]-3; mpos[0]=pos[0]+vel[0]*0; mpos[1]=pos[1]+vel[1]*0; mpos[2]=pos[2]+vel[2]*0; cam_pos_x=cam[1]; cam_pos_y=-cam[2]; cam_pos_z=cam[0]; // standard smooth lookat camera raydium_camera_look_at(cam[0],cam[1],cam[2],mpos[1],-mpos[2],mpos[0]); }
Vehicule_commande :
Cette fonction permet de définir les commandes permettant de contrôler le drone au clavier. Cette fonction n’est active que lorsque le mode de pilotage par la liaison série est désactivé, c’est à dire que la variable MODE est à ‘0’.
Le moteur du drone est piloté par une commande tout ou rien, la mise en marche du moteur se fait par un appui sur la touche '0' et l’arrêt de celui-ci par la touche '.'.
Les touches '4' et '6' permettent d’imposer un angle au palonnier de l’empennage vertical, ce qui permet au drone de faire de petits virages à plat.
Les flèches de direction permettent de contrôler le drone.
Une valeur est appliquée par défaut sur les palonniers des ailes ainsi que sur celui de l’empennage horizontal pour permettre au drone de voler à une hauteur fixe sans agir sur les commandes. Ces valeurs ne seront pas appliquées lors d’un pilotage par la liaison série.
{ if(MODE == 0){ //mise en marche du moteur avec la touche 1 if (raydium_key_last==1000+'0' || raydium_key_last==1000+'0') mot=1; //arret du moteur avec la touche 2 if (raydium_key_last==1000+'.' || raydium_key_last==1000+'.') mot=0; // commande du palonier vertical de la queue grace aux touches 4 et 5 // l'effet sur le palonier n'est applique que lors de l'appui // lorsque les touches sont relachees, le palonier revien droit if (raydium_key_last==1000+'4' || raydium_key_last==1000+'6'){ if (raydium_key_last==1000+'4' || raydium_key_last==1000+'4') alpha0emp1=-0.1; if (raydium_key_last==1000+'6' || raydium_key_last==1000+'6') alpha0emp1=0.1; } else alpha0emp1=0; //commande du palonier arriere horizontal avec les touches haut, bas alpha0emp=raydium_joy_y*0.2-0.01;//-0.25; //commande des paloniers des ailes gauche et droite avec les touche //doite et gauche. //pour piloter la direction du drone les angles appliques sur les //paloniers doivent etre inverse alpha0aile2=raydium_joy_x*0.05;//+0.06; alpha0aile1=-raydium_joy_x*0.05;//+0.06; } }
Clavier:
Cette fonction permet de définir les commandes du clavier servant à changer de mode ou encore à réinitialiser la simulation.
Les touche ‘a’,’z’ et ’e’ permettent de régler la vitesse du simulateur.
if(raydium_key_last==1000 + 'a') raydium_ode_time_change(0); if(raydium_key_last==1000 + 'z') raydium_ode_time_change(10); if(raydium_key_last==1000 + 'e') raydium_ode_time_change(100);
La touche ‘r’ permet de remettre le drone à la verticale à l' endroit où il se trouve.
if (raydium_key_last==1000+'r'){ raydium_ode_object_rotate_name_3f("drone",0,PI/2,0);}
La touche espace permet de réinitialiser le simulateur, c'est-à-dire qu’elle rappelle les fonctions de création du simulateur.
if(raydium_key_last==1032) { mot=0; timer=0; raydium_ode_object_delete_name("drone"); simul_create(); raydium_ode_ground_set_name(piste); }
La touche 'b' permet de créer une boite non texturée à des fins de tests de collisions et de poid. Ces boites ne sont pas détruites et restent donc, pouvant causer des ralentissements s'il y en a trop.
Pour les voir, un appuis sur 'd' est indispensable.
if (raydium_key_last==1000+'b'){ int a; char name[255]; a=raydium_ode_object_find("GLOBAL"); raydium_ode_name_auto("box",name); raydium_ode_object_box_add(name,a,50,5,5,5,RAYDIUM_ODE_STANDARD,0,""); raydium_ode_element_move_name_3f(name,0,400,12); raydium_ode_element_material_name(name,gerp,gcfm); raydium_ode_element_material_name("ground",gerp,gcfm); }
Les touches ‘s’ et ‘l’ permettent de basculer d’un mode de caméra à un autre.
//passe la camera en mode suivit du drone if ((raydium_key_last==1000+'s') || (raydium_key_last==1000+'S')) mode_camera=suivi; //passe la camera en mode libre if ((raydium_key_last==1000+'l') || (raydium_key_last==1000+'L')) mode_camera=libre;
Les touches ‘d’ et ‘i’ permettent respectivement d’afficher les éléments constituant le model physique et d’afficher les informations concernant la liaison série.
//affiche les element constituant le model physique du drone if ((raydium_key_last==1000+'d') || (raydium_key_last==1000+'D')) mode_affichage=(mode_affichage==normal)? debug:normal; //affiche les informations concernant la liaison serie if ((raydium_key_last==1000+'i') || (raydium_key_last==1000+'I')) { affichage_information++; if (affichage_information>rien) affichage_information=0; }
Chrono :
Fonction permettant de calculer une base de temps depuis la dernière réinitialisation du simulateur. Cette fonction est appelée toutes les millisecondes.
void chrono() { timer+= 0.01; }
Display :
Cette fonction permet de régler les options d’affichage, telles que la position de la lumière et de la caméra. Nous utilisons aussi cette fonction pour afficher les informations liées au drone telles que sa position, sa vitesse ou encore les angles appliqués sur les volets de direction.
raydium_osd_printf(75,60,16,0.5,"font2.tga","vitesse2 : %.1f",vitesse2); raydium_osd_printf(75,57,16,0.5,"font2.tga"," vitesse : %1.2f", vitesse); raydium_osd_printf(75,54,16,0.5,"font2.tga"," alphay : %1.2f",alphay); raydium_osd_printf(75,51,16,0.5,"font2.tga"," alphaz : %1.2f",alphaz); raydium_osd_printf(75,48,16,0.5,"font2.tga"," localv1 : %1.2f", localv[1]); raydium_osd_printf(75,45,16,0.5,"font2.tga"," localv2 : %1.2f", localv[2]); raydium_osd_printf(2,78,16,0.5,"font2.tga"," Traction (X) : %f", traction*251); raydium_osd_printf(2,76,16,0.5,"font2.tga"," Trainee (-X) : %f", traine*1000); raydium_osd_printf(2,74,16,0.5,"font2.tga"," portanceZaile1 (Z) : %f", portanceZaile1*1000); raydium_osd_printf(2,72,16,0.5,"font2.tga"," portanceZaile2 (Z) : %f", portanceZaile2*1000); raydium_osd_printf(75,74,16,0.5,"font2.tga","aile droite : %1.2f",alpha0aile1); raydium_osd_printf(75,71,16,0.5,"font2.tga","aile gauche : %1.2f",alpha0aile2); raydium_osd_printf(75,68,16,0.5,"font2.tga","emp hauteur : %1.2f",alpha0emp); raydium_osd_printf(75,65,16,0.5,"font2.tga","emp direc : %1.2f",alpha0emp1); raydium_osd_printf(55,62,16,0.5,"font2.tga","mot : %d / traction : %1.2f",mot,traction); vehicule_commande(vehicule); raydium_osd_printf(2,2,16,0.5,"font2.tga","Chrono: %3.1f", timer); raydium_osd_printf(75,60,16,0.5,"font2.tga","vitesse2 : %.1f",vitesse2); raydium_osd_printf(75,57,16,0.5,"font2.tga"," vitesse : %1.2f", vitesse); raydium_osd_printf(75,54,16,0.5,"font2.tga"," alphay : %1.2f",alphay); raydium_osd_printf(75,51,16,0.5,"font2.tga"," alphaz : %1.2f",alphaz); raydium_osd_printf(75,48,16,0.5,"font2.tga"," localv1 : %1.2f", localv[1]); raydium_osd_printf(75,45,16,0.5,"font2.tga"," localv2 : %1.2f", localv[2]); raydium_osd_printf(2,78,16,0.5,"font2.tga"," Traction (X) : %f", traction*251); raydium_osd_printf(2,76,16,0.5,"font2.tga"," Trainee (-X) : %f", traine*1000); raydium_osd_printf(2,74,16,0.5,"font2.tga"," portanceZaile1 (Z) : %f", portanceZaile1*1000); raydium_osd_printf(2,72,16,0.5,"font2.tga"," portanceZaile2 (Z) : %f", portanceZaile2*1000);
Afin de mieux visualiser les forces appliquées au drone, celles ci sont affichées sur le drone sous la forme de vecteur.
dReal *v = dBodyGetLinearVel(bodycorp); dReal *p =dBodyGetPosition(bodycorp); dReal vg[4]; raydium_draw_line(p[0],p[1],p[2],p[0]+v[0]*0.5,p[1]+v[1]*0.5,p[2]+v[2]*0.5,0,1,0); //affichage des differentes forces appliquees sur le drone p=dBodyGetPosition(bodyaile1); dBodyVectorToWorld(bodyaile1,0,0,portanceZaile1*3,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,0); p=dBodyGetPosition(bodyaile2); dBodyVectorToWorld(bodyaile1,0,0,portanceZaile2*3,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,0); p=dBodyGetPosition(bodycorp); dBodyVectorToWorld(bodycorp,0,portanceYcorps*3,0,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],0,1,1); p=dBodyGetPosition(bodyemp); dBodyVectorToWorld(bodyemp,0,0,portanceZemp*3,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,0); p=dBodyGetPosition(bodyemp); dBodyVectorToWorld(bodyemp,0,portanceYemp*3,0,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,0); p=dBodyGetPosition(bodyemp); dBodyVectorToWorld(bodyemp,-traine*3,0,0,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,0); p=dBodyGetPosition(bodytraction); dBodyVectorToWorld(bodytraction,traction*3,0,0,vg); raydium_draw_line(p[0],p[1],p[2],p[0]+vg[0],p[1]+vg[1],p[2]+vg[2],1.0f,0,1);
Les fonctions de réglage de la caméra ainsi que l’éclairage de la carte sont déjà expliqués en grande partie sur les Wiki du site Raydium (RaydiumTutoriels).
Nous avons également utilisé les Pixels Shader pour représenter une étendue d’eau sur la carte. Le code permettant de réaliser ce rendu a été repris du programme Volcano présent dans les exemples du site de Raydium et ne sera pas commenté car il ne présente pas vraiment d’intérêt pour notre projet…
Periph_write :
Cette fonction a pour but de lire des informations depuis la liaison série, telles que l’angle des ailes ou la position du drone. Ces informations ont pour but de piloter le drone depuis la liaison série.Son implémentation est détaillé dans la partie : SimulCapteurs
Periph_read :
Cette fonction a pour but d’envoyer des informations par la liaison série, telles que la position du drone ou encore sont orientation par rapport à la normale. Ces informations ont pour but de connaitre les informations concernant l’évolution du drone pour réaliser un asservissement de celui-ci.Sont implémentation est détaillé dans la partie : SimulCapteurs
Step :
Cette fonction est la seule qui est effectuée à chaque pas du moteur physique (ODE). Nous avons appelé notre fonction de calcul et d’application des forces à l’objet au début de cette fonction (fonction détaillée en-dessous).Le code présent dans cette fonction sert à récupérer les informations présentes sur la liaison série ou à en envoyer. Nous n’avons pas écrit cette partie du code, nous avons réutilisé des fonctions utilisées lors de TP.
Simul_init :
Cette fonction permet d’initialiser le port série utilisé pour la communication entre le simulateur et le microcontrôleur. Les paramètres du port série sont les suivants :115200 bauds, 8 bit de donné, sans parité et un seul bit de stop
{ char pn[32]; raydium_init_cli_option_default("com",pn,"com2:"); com=comm_init (pn,115200,8,NOPARITY,ONESTOPBIT); }
Calculs :
Cette fonction réalise l’ensemble des calculs des forces régissant le système et les applique au model physique. Celle-ci est réalisée à chaque pas du moteur physique.
Les équations physiques ont été déterminées sur la page EquaPhys.
Le 1er calcul réalisé est celui permettant de déterminer les angles de rotation qu’a subi le drone par rapport à la normale. L’ensemble des détails concernant ces calculs ou leurs indentations se trouve à la page suivante : SimulCapteurs
//permet de recuperer la vitesse relative au drone en x y et z //grace a des fonctions ODE dReal *v = dBodyGetLinearVel(bodycorp); dBodyVectorFromWorld(bodycorp, v[0], v[1], v[2], localv);
Ceci est réalisé grâce à des fonctions primitives d’ODE permettant de récupérer les vecteurs liés à un objet (dBodyVectorFromWorld).
Pour avoir la vitesse relative du drone, il suffit de récupérer la valeur de localv[0].
vitesse = localv[0]; // calcul la vitesse globale du drone au carre vitesse2 = vitesse * vitesse;
Il nous faut aussi les angles entre les vecteurs vitesse de chaque axe et le sol. Ceci entre en compte dans les équations permettant de calculer les portances. (Voir EquaPhys)
Ces angles sont déterminés d’après de simples équations trigonométriques étant donné que les vecteurs vitesse sont orthogonaux.
// calcul de l'angle entre le vecteur vitesse du drone et sont // inclinaison par rapport au sol dans le plan X/Z //localv[2] correspond au roulie et tangage alphaz = atan2(localv[2],localv[0]); //calcul de l'angle entre le vecteur vitesse du drone et sont // inclinaison par rapport au sol dans le plan X/Y //localv[1] correspond au cap alphay = atan2(localv[1],localv[0]);
Le calcul suivant détermine la force de traction à appliquer au drone. Ces calculs permettent d’implémenter la courbe de traction déterminée précédemment. (Voir EquaPhys)
Cette force n’aura de valeur que lorsque la variable « mot » et à 1 ce qui signifie que le moteur est démarré.
// si la traction doit etre appliquee effectuer les calculs de la force if(mot == 1) traction = ((1.3 * 0.7) * ( VITESSE_DRONE_MAX - (coef_moteur * vitesse))); // si la traction ne doit pas etre appliquée elle vaut 0 else traction = 0; dBodyAddRelForce(bodytraction,traction*1000,0,0);
La suite de cette fonction est le code permettant de calculer et d’appliquer les forces au drone. Ces forces sont calculées d’après la formule des portances déterminées auparavant. (Voir EquaPhys)
Les forces appliquées sont préalablement divisées par un coefficient défini au départ, pour homogénéiser les réactions du système.
{// calcul de la portance a appliquer a l'aile droite en Z if(((alphaz + alpha0aile1/3)) < PI/9) { portanceZaile1 = (-1.25/2)*COEF_PORTANCE_DEMI_AILE_Z* (alphaz + alpha0aile1/3)*SURFACE_PORTANTE_DEMI_AILE_Z*vitesse2; } else { portanceZaile1 = 0; } //applique la portance calcule precedement dBodyAddRelForce(bodyaile1,0,0,portanceZaile1*1000.0f); } {// calcul de la portance a appliquer a l'aile gauche en Z if(((alphaz + alpha0aile2/3)) < PI/9) { portanceZaile2 = (-1.25/2)*COEF_PORTANCE_DEMI_AILE_Z* (alphaz + alpha0aile2/3)*SURFACE_PORTANTE_DEMI_AILE_Z*vitesse2; } else { portanceZaile2 = 0; } //applique la portance calcule precedement dBodyAddRelForce(bodyaile2,0,0,portanceZaile2*1000.0f); } {// calcul de la portance a appliquer a l'empennage en Z if((alphaz + alpha0emp/3) < PI/4) { portanceZemp = (-1.25/2)*COEF_PORTANCE_EMPENNAGE_Z*(alphaz + alpha0emp/3)*SURFACE_PORTANTE_EMPENNAGE_Z*vitesse2; } else { portanceZemp = ((-1.25/2)*COEF_PORTANCE_EMPENNAGE_Z*(PI/4)*SURFACE_PORTANTE_EMPENNAGE_Z*vitesse2) - ((-1.25/2)*COEF_PORTANCE_EMPENNAGE_Z*((alphaz + alpha0emp/3)-PI/9)*SURFACE_PORTANTE_EMPENNAGE_Z*vitesse2); } //applique la portance calcule precedement dBodyAddRelForceAtRelPos (bodyemp,0,0,portanceZemp*1000.0f,0,0,0); } {// calcul de la portance a appliquer au corps en Y portanceYcorps = (1.25/2)*COEF_PORTANCE_CORPS_Y*(-alphay)* SURFACE_PORTANTE_CORPS_Y*vitesse2; } //applique la portance calcule precedement dBodyAddRelForce(bodycorp,0,portanceYcorps*1000.0f,0); {// calcul de la portance a appliquer a l'empennage en Y portanceYemp = (-1.25/2)*COEF_PORTANCE_EMPENNAGE_Y* (-alphay + alpha0emp1/3)*SURFACE_PORTANTE_EMPENNAGE_Y*vitesse2; //applique la portance calcule precedement dBodyAddRelForce(bodyemp,0,portanceYemp*1000.0f,0); } //calcul de la traine par une equation lineaire simple, ce modele n'est pas realiste mais simplifie traine = COEF_TRAINE*vitesse; dBodyAddRelForce(bodyemp,-traine*1000.0f,0,0);
Les 2 lignes de code suivantes permettent, lorsque les commandes sont envoyées par la liaison série, la remise à zéro des angles appliqués sur les palonniers des ailes. Ceci permet d’appliquer les angles envoyés une seule fois, étant donné que le simulateur est beaucoup plus rapide que le microcontrôleur.
//permet d'eviter de partir en vrille if(MODE == 1){ alpha0aile1=0; alpha0aile2=0; }
Main :
Le début du main correspond à un programme principal générique de raydium, taille de la fenêtre, réglage des luminosités, …etc… . tout ceci étant expliqué dans les tutoriaux du site de raydium. (voir RaydiumTutoriels )Il y a ensuite le code permettant de créer les « shaders » pour l’affichage de l’eau.
Nous avons ensuite utilisé une fonction Raydium qui permet un appel d’une fonction à un temps donné (pour nous 100ms) afin de gérer le chrono.
Les deux autres fonctions servent à créer le sol en fonction du model « .tri » passé en paramètre au début du programme.
Il faut aussi définir la gravité, ici nous avons opté pour une gravité proche de la réalité : 9.81.
raydium_ode_ground_set_name(piste); raydium_ode_element_slip_name("ground",RAYDIUM_ODE_SLIP_DEFAULT/100.0f); raydium_ode_element_material_name("ground",gerp,gcfm); raydium_ode_gravity_3f(0,0,-9.81); //definition de la gravite
Le main se finit par l’appel des différentes fonctions qui initialisent et lancent le simulateur.
Et enfin nous définissons les fonctions appelées à chaque pas d’ODE ainsi que celle gérant l’affichage.
simul_init(); simul_create(); raydium_ode_StepCallback=step; raydium_callback(&display);
Retour