====== Carte de vol : HePilot ======

La politique de l'équipe étant de créer notre drone de A à Z, il était tout naturel de ne pas acheter une carte de vol déjà assemblée. Cela nous a permis de faire d'énormes économies tout en restant compétitifs.
Si vous aussi vous vous intéressez à la création de drone, voici de A à Z le fonctionnement de la HePilot 1.0.

==== Hardware ====

=== Composants ===

La carte HePilot est composée d'un micro-controlleur Arduino uno, d'une centrale inertielle (IMU) 9 degrés de liberté MPU-9150, d'un shield xbee avec le module qui va bien (xbee pro) et de quelques résistances.

{{:wiki:projets:godhelium:hepilotschema.jpg?500|}}

Elle permet de :
  * Stabiliser le drone
  * Recevoir des commandes de la GCS
  * Envoyer des informations sur l'état du drone à la GCS

=== Montage ===

L'IMU, et les résistances sont directement soudées sur la partie "prototype" du shield XBee, avec quelques connecteurs et des ponts en fils, il est simple de reproduire le schéma ci-dessus quand l'on a de l'étain de bonne qualité.

{{:wiki:projets:godhelium:2014-02-06_18.18.22.jpg?500|}}

Dans le schéma de fonctionnement, deux parties nécessitent des explications. L'IMU communique avec l'Arduino à l'aide du protocole I2C et a donc besoin de deux résistances appelées "pull up", Les résistances utilisées sont des 2.2 kΩ. 

Le deuxième point important de la carte est le circuit de lecture de tension pour la batterie. En effet il était capital de surveiller la charge de notre batterie pour pouvoir poser le drone avant qu'il ne tombe tout seul. Pour cela nous utilisons une entrée analogique de l'Arduino comme un voltmètre car la tension délivrée par la batterie dépend directement de la charge. Petit problème : notre batterie est une 3S ce qui signifie qu'elle délivre une tension comprise entre 9 et 12 volts, tandis que l'entrée analogique lit des valeurs comprises entre 0 et 5 volts. Nous avons donc monté un diviseur de tension qui nous permet de diviser la tension de notre batterie par 3, il suffit ensuite de re-multiplier notre valeur dans le code, mais ce travail est effectué par la GCS.

=== Configuration des XBees ===

Pour configurer les 2 XBees, nous avons utilisé notre xbee explorer et le logiciel gratuit Coolterm.
La première étape à été de relever les adresses des deux modules écrites au dos de ceux-ci. Ensuite nous les avons configurer de tel sorte que :

  * ils aient le même baud rate
  * ils soient sur le même réseau
  * chacun connaisse l'adresse de l'autre

Pour cela nous avons utilisé les commandes suivantes :

<code>
+++ // Rentre en mode configuration
ATID01 // Réseau 01
ATDH13A200 // Partie haute de l'adresse de destination
ATDL408CBB9F // Partie basse de l'adresse de destination
ATBD3 // Baud rate de 9600
ATWR // On oublie pas de sauvegarder les changements
</code>
==== Programmation de la HePilot ====

Avant de se lancer dans le code, nous allons essayer de comprendre le fonctionnement du code de guidage / stabilisation.

=== Définition du drone ===

{{:wiki:projets:godhelium:drone_schema.png?500|}}

Le pilote envoie 4 commandes au drone :
  * Puissance (Thrust) : Pour monter / accélérer
  * Lacet (Yaw) : Pour tourner sur soi-même
  * Tangage (Pitch) : Pour avancer / reculer
  * Roulis (Roll) : Pour aller à gauche / à droite

Par convention, une valeur positive voudra dire : monter, tourner dans le sens horaire, avancer, aller à droite

Nous devons donc convertir ces 4 commandes (P, L, T, R) en informations de vitesse utilisables par les moteurs:

  * M1 = P - L - T
  * M2 = P + L - R
  * M3 = P - L + T
  * M4 = P + L + R

=== Priorité des tâches ===

La carte ne devra pas seulement s’occuper du vol, mais aussi de toutes les fonctionnalités additionnelles. Certaines tâches sont moins importantes que d’autres et/ou ne nécessitent pas d’être exécutées aussi souvent.

  * Grande priorité :
    * Récupération des données de l’IMU
    * Calcul des PID et update des moteurs
  * Moyenne priorité :
    * Récupération des commandes envoyées par le pilote
  * Basse priorité :
    * Calcul de la charge de la batterie
    * Envoi des informations de l’IMU au pilote

  * 1 µs = 0.000001 s
  * 100Hz -> 0.01s = 10000 µs (Grande priorité)
  * 50Hz -> 0.02s = 20000 µs (Moyenne priorité)
  * 10Hz -> 0.1s = 100000 µs (Basse priorité)

=== Contrôleurs PID ===

Vous en avez peut-être déjà entendu parler au cours de vos recherches, il peuvent être qualifiés de stabilisateurs de vol. Je vais ici essayer d’expliquer le principe et le fonctionnement des contrôleurs PID.

== Qu’est ce que c’est ? ==

Un contrôleur PID (Proportionnel, Intégrale, Dérivée) est un système fermé dont le but est de s’approcher au plus près d’un résultat voulu, en se basant sur des données de capteurs. Pour le drone, ils sont en charge d’assurer la stabilité, il en faudra donc un par axe de rotation (lacet, roulis, tangage).

Le principe est en fait assez simple : 3 algorithmes sont combinés en fonction de coefficients kP, kI et kD qui déterminent le style de vol de l’appareil.

== Calcul de P, I et D ==

Nos 3 algos ont besoins de 2 entrées : la valeur voulue, dite de référence (ref), c’est celle que le pilote demande et la valeur réelle (input), détectée par l’IMU (Inertial Measurement Unit).

Des 3 algos, le plus simple est l’algo proportionnel, ce qui tombe bien car c’est aussi le plus important !

<code=cpp>P = kP * (ref - input)</code>

L’algo d’intégration a pour but de garder une trace des petites erreurs de positionnement commises tout au long du vol, sa valeur dépend donc de ses valeurs précédentes. Au début du vol, I = 0.

<code=cpp>I += kI * (ref - input) * dt</code>

dt est l'intervalle de temps qui s’est écoulé entre 2 calculs de I, ce paramètre intervient aussi lors du calcul suivant.

L’algo de dérivation, contrairement au précédent essaie d’anticiper les erreurs futures :

<code=cpp>D = - kD * (input - prevInput) / dt</code>

Où prevInput est la valeur de input qui a servi à calculer la valeur précédente.

Pour finir, on fait la somme de ces 3 algorithmes :

<code=cpp>return P + I + D;</code>

Et notre valeur est prête à être utilisée dans nos équations de moteurs (voir plus haut).

== Influence des coefficients sur le vol ==

Proportionnel : le plus important, plus il est grand, plus le drone réagira rapidement. Par contre s’il est trop élevé, il sera trop sensible aux petites oscillations.

Intégrale : augmente la précision. Il est très utile pour contrer le vent et la turbulence des moteurs.

Dérivée : plus il est élevé, plus le drone sera sensible au moindre changement. Attention à ne pas trop en abuser.

Quelques exemples de configurations trouvées sur internet :

Vol acrobatique :
kP un peu plus élevé
kI un peu moins élevé
kD plus élevé


Vol stable :
kP un peu moins élevé
kI un peu plus élevé
kD moins élevé

== Réglage de PID via la gcs ==

Nous devrions profiter de l’avantage que nous avons, d’avoir opté pour une commande PC et lui ajouter la possibilité de changer les paramètres kP, kI et kD de chaque axe via la liaison sans fil. Cela permettra de faire des tests sans avoir à reprogrammer le drone et de changer de configuration (acrobatique / stable) simplement pendant l’épreuve.

== Conclusion ==

Les quadricoptères étant symétriques, ont peut utiliser les mêmes coefficients kP, kI et kD pour l’axe de tangage et de roulis. L’axe de lacet, n’est pas primordial pour garder l’appareil en l’air, donc, est plus simple à régler. Aussi, on devrait utiliser 2 contrôleurs PID pour les axes de tangage et de roulis, un pour la vitesse (accéléromètre) et un pour la rotation (gyroscope), on les combine ensuite de la sorte : <code=cpp>pid_vit(pid_rot(ref_roll, gyroRoll), accelDroite);</code>

=== Protocole de communication ===

Nous avons opté pour un protocole de communication simple et efficace. Afin de réduire considérablement la taille des paquets échangés, tout en gardant une précision acceptable, nous avons décider d'encoder toutes nos valeurs en binaire. Nos valeurs étant des entiers flottant, il nous faut 4 octets pour en encoder une.

Nous divisons les paquets de notre protocole en deux catégories, selon leur sens.

GCS -> Drone (Paquets de commande)
  * T{valeur} (Thrust / Puissance)
  * Y{valeur} (Yaw / Lacet)
  * P{valeur} (Pitch / Tangage)
  * R{valeur} (Roll / Roulis)
  * K{valeur}{valeur}{valeur} (Coefficients PID)

Drone -> GCS (Paquets d'information)
  * {yaw}{pitch}{roll}{ax}{ay}{az}{tension batterie}

Nous utilisons une astuce de code C / Arduino pour recomposer notre entier flottant à partir des 4 octets :

<code=cpp>
union btf {
  float f;
  byte b[4];
} byteToFloat;

// Lecture des octets
byteToFloat.b[3] = Serial.read();
byteToFloat.b[2] = Serial.read();
byteToFloat.b[1] = Serial.read();
byteToFloat.b[0] = Serial.read();

// Notre valeur flottante :
float value = byteToFloat.f;
</code>

=== Code final commenté ===

<code=cpp>
/*
  * HePilot
  * Team Helium - Game of Drones
  * Author: Arthur Hennequin
  */

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_9Axis_MotionApps41.h"
#include "PID.h"
#include <Servo.h> 

#define PIN_M1 6
#define PIN_M2 9
#define PIN_M3 10
#define PIN_M4 11

#define PIN_VCC 2
#define DEBUG

// Controlleurs PID
PID pid_yaw(1, 0, 0); // Lacet
PID pid_pitch(1, 0, 0); // Tangage
PID pid_roll(1, 0, 0); // Roulis

// Moteurs
Servo m1;
Servo m2;
Servo m3;
Servo m4;

// IMU
MPU6050 imu;

bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

float ypr[3];
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity;

float prevYaw = 0;
float prevPitch = 0;
float prevRoll = 0;
float yaw = 0;
float pitch = 0;
float roll = 0;

float vX = 0.;
float vY = 0.;
float vZ = 0.;

float vcc = 0.;

// Commandes
float ref_thrust = 0; // Puissance
float ref_yaw = 0; // Lacet
float ref_pitch = 0; // Tangage
float ref_roll = 0; // Roulis

char type;
union btf {
  byte b[4];
  float f;
} byteToFloat;

// Gestion du temps
long keepAliveTime;
long currentTime;
long lastTime;
long dT;
int frameCounter;

void setup() {
  Wire.begin();
  
  Serial.begin(9600); // Ouvre la connexion xBee ou série
  
  // Init motors
  pinMode(PIN_M1, OUTPUT);
  pinMode(PIN_M2, OUTPUT);
  pinMode(PIN_M3, OUTPUT);
  pinMode(PIN_M4, OUTPUT);
  
  m1.attach(PIN_M1);
  m2.attach(PIN_M2);
  m3.attach(PIN_M3);
  m4.attach(PIN_M4);
  
  // Calibration
  m1.write(170); // Max
  m2.write(170);
  m3.write(170);
  m4.write(170);
  
  delay(5000); // Brancher ESCs
  
  m1.write(10); // Min
  m2.write(10);
  m3.write(10);
  m4.write(10);
  
  delay(5000);
  
  // Init IMU
  #ifdef DEBUG
  Serial.println("InitIMU");
  #endif
  imu.initialize();
  devStatus = imu.dmpInitialize();
  
  if (devStatus == 0) {
    #ifdef DEBUG
    Serial.println("IMU ok");
    #endif
    imu.setDMPEnabled(true);
    mpuIntStatus = imu.getIntStatus();
    dmpReady = true;
    packetSize = imu.dmpGetFIFOPacketSize();
  }
  
  imu.resetFIFO();
  lastTime = micros();
  keepAliveTime = millis();
}

float lerp(float a, float b, float r) {
  return a*r + b*(1-r);
}

void readIMUData() {
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (fifoCount < packetSize) {
    fifoCount = imu.getFIFOCount();
  }
  
  mpuIntStatus = imu.getIntStatus();
  
  // get current FIFO count
  fifoCount = imu.getFIFOCount();
  
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      imu.resetFIFO();
      #ifdef DEBUG
      Serial.println("OVF");
      #endif
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize)
      fifoCount = imu.getFIFOCount();

    // read a packet from FIFO
    imu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;
          
    // display Euler angles in degrees
    imu.dmpGetQuaternion(&q, fifoBuffer);
    imu.dmpGetGravity(&gravity, &q);
    imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    
    #ifdef DEBUG
    Serial.println("ypr");
    #endif
    
    //#define kFilteringFactor 0.5
    //yaw = lerp(ypr[0]*180/M_PI, prevYaw, kFilteringFactor);
    //pitch = lerp(ypr[1]*180/M_PI, prevPitch, kFilteringFactor);
    //roll = lerp(-ypr[2]*180/M_PI, prevRoll, kFilteringFactor);
    
    yaw = ypr[0]*180/M_PI;
    pitch = ypr[1]*180/M_PI;
    roll = -ypr[2]*180/M_PI;
    
    //prevYaw = yaw;
    //prevPitch = pitch;
    //prevRoll = roll;
  }
}

float correctAngle(float angle) {
  if (angle>360) {
    return angle - 360;
  } else if (angle<0) {
    return angle + 360;
  }
  return angle;
}

float clamp(float value, float a, float b) {
  return min(max(value, a), b);
}

void updateMotors(float dt) {
  float Y = pid_yaw.compute(ref_yaw, yaw, dt);
  float P = pid_pitch.compute(ref_pitch, pitch, dt);
  float R = pid_roll.compute(ref_roll, roll, dt);
  
  if ((millis() - keepAliveTime) <= 2000 && ref_thrust > 0.f) {
    m1.write(map(clamp(ref_thrust - Y + P, 0, 100), 0, 100, 10, 170));
    m2.write(map(clamp(ref_thrust + Y + R, 0, 100), 0, 100, 10, 170));
    m3.write(map(clamp(ref_thrust - Y - P, 0, 100), 0, 100, 10, 170));
    m4.write(map(clamp(ref_thrust + Y - R, 0, 100), 0, 100, 10, 170));
  } else {
    m1.write(10);
    m2.write(10);
    m3.write(10);
    m4.write(10);
  }
}

float readSerialFloat() {
  byteToFloat.b[3] = Serial.read();
  byteToFloat.b[2] = Serial.read();
  byteToFloat.b[1] = Serial.read();
  byteToFloat.b[0] = Serial.read();
  return byteToFloat.f;
}

void readCommands() {
  if (Serial.available()>=5) {
    type = Serial.read();
    if (type=='T') {
      ref_thrust = readSerialFloat();
    } else if (type=='Y') {
      ref_yaw = readSerialFloat();
    } else if (type=='P') {
      ref_pitch = readSerialFloat();
    } else if (type=='R') {
      ref_roll = readSerialFloat();
    } else if (type=='K') {
      float kP = readSerialFloat();
      float kI = readSerialFloat();
      float kD = readSerialFloat();
      pid_yaw.setCoeffs(kP, kI, kD);
      pid_pitch.setCoeffs(kP, kI, kD);
      pid_roll.setCoeffs(kP, kI, kD);
    } else if (type=='Z') {
      keepAliveTime = millis();
    }
  }
}

void monitorBattery() {
  vcc = analogRead(PIN_VCC) / 1023. * 5.;
}

void writeSerialFloat(float f) {
  byteToFloat.f = f;
  Serial.write(byteToFloat.b[3]);
  Serial.write(byteToFloat.b[2]);
  Serial.write(byteToFloat.b[1]);
  Serial.write(byteToFloat.b[0]);
}

void sendIMUData() {
  Serial.write(0xBE);
  writeSerialFloat(yaw);
  writeSerialFloat(pitch);
  writeSerialFloat(roll);
  writeSerialFloat(vcc);
}

void loop() {
  currentTime = micros();
  dT = currentTime - lastTime;
  
  if (dT >= 10000) { // 100 Hz = 10ms
    frameCounter++;
    
    // 100 Hz - Grande priorité
    readIMUData();
    updateMotors(dT * 0.000001); // dT en secondes
    
    // 50 Hz - Moyenne priorité
    if (frameCounter%2) {
      readCommands();
    }
    
    // 25 Hz - Basse priorité
    if (frameCounter%4) {
      monitorBattery();
      sendIMUData();
    }
    
    lastTime = currentTime;
    
    if (frameCounter==100)
      frameCounter = 0;
  }
}
</code>

Classe PID.h :

<code=cpp>
#ifndef PID_h
#define PID_h

#include "Arduino.h"

class PID {
  public:
    PID(float p, float i, float d) {
      kP = p;
      kI = i;
      kD = d;
      
      iTerm = 0;
    }
    
    void reset() {
      iTerm = 0;
    }
    
    float compute(float ref, float input, float dt) {
      float error = ref - input;
      
      pTerm = error;
      iTerm += error * dt;
      dTerm = (input - prevInput) / dt;
      
      prevInput = input;
    
      return kP*pTerm + kI*iTerm - kD*dTerm;
    }
    
    void setCoeffs(float p, float i, float d) {
      kP = p;
      kI = i;
      kD = d;
      
      iTerm = 0;
    }
  
  private:
    float prevInput;
    float pTerm, iTerm, dTerm;
    float kP, kI, kD;
};

#endif
</code>
==== Programmation de la GCS ====

A rédiger