Utiliser sa radiocommande avec un Arduino - drone ch.6

Bonjour à tous,

Dans ce nouveau chapitre nous allons voir comment utiliser une radiocommande et son récepteur et comment décoder les instructions reçues avec un Arduino pour pouvoir les exploiter dans le contexte de notre drone.

On monte d'un cran en terme de complexité alors accrochez-vous à vos slibards !

1. Comprendre sa radiocommande

La première fois qu'on manipule une vraie radiocommande, ça impressionne un peu. Beaucoup de boutons, un menu assez complexe bref, pas simple de s'y retrouver au début.

La meilleure chose à faire pour commencer est de lire la doc fournie. Le cas échéant de fouiner sur le net en quête d'info.

1.1 Les différents modes

Comme nous avons chacun nos préférences, certains voudront avoir la commande des gaz à droite et d'autres à gauche.

Pour contenter tout le monde, il existe plusieurs modes qui assignent les commandes à différents sticks de la RC. Il en existe 4 principaux, comme le montre cette figure:

Remote modes
Les différents modes

Habituez-vous dès maintenant à utiliser les termes anglais car je les utiliserai par la suite.

À vous de voir avec lequel vous êtes le plus à l'aise. Il s'agit d'un choix relativement important dans la mesure où il en va de votre confort de pilotage et donc de la survie de votre drone.

Lorsque vous pilotez le drone de quelqu'un d'autre, demandez-lui quel mode il utilise. Ça vous évitera de subir son courroux parce que vous avez confondu les gaz avec le tangage 😛

1.2 Exemple de la FlySky FS-i6

Pour ma part, j'ai investi dans la FlySky FS-i6, livrée avec son récepteur. Cette radiocommande dispose de 6 canaux, de 4 switchs, d'un écran LCD et de boutons permettant de naviguer dans les menus de configuration.

FlySky FS-i6
FlySky FS-i6 et son récepteur

Le manuel utilisateur est fourni sur CD, mais reste assez sommaire et ne rentre pas dans les détails des menus.

Après avoir fouillé dans les menus, expérimenté et cherché sur le net, voici un rapide résumé:

Les 4 switchs permettent de passer d'une configuration préenregistrée à une autre rapidement. Par exemple, passer d'un mode standard à un mode sport, ou encore mode vol et mode atterrissage.

Lorsqu'on modifie la config, il faut maintenir enfoncé le bouton "cancel" pour sauvegarder. Je sais, c'est contre-intuitif 😕

On peut visualiser la commande de chaque canal sous forme d'histogrammes dans un menu dédié, ce qui est assez pratique pour identifier l'association stick/canal.

On peut également configurer le fail-safe. J'en reparlerai lorsqu'on passera au premier vol mais pour faire court, il s'agit des consignes par défaut que le récepteur doit appliquer en cas de perte de signal.
Perso, j'ai réglé le fail-safe sur "throttle = 0", ce qui coupe immédiatement les moteurs. Le drone se crash immédiatement mais au moins ne s'en va pas dans la nature, au risque de s'écraser sur une voiture ou un piéton ☹️

Cette partie n'a pas vocation à faire une présentation exhaustive de cette radiocommande. Je vous présente seulement les points que je juge intéressants.

2. Appairer émetteur et récepteur (optionnel) 

Pour que l'émetteur et le récepteur communiquent, ils doivent d'abord être appairés. Lorsque vous achetez une radiocommande livrée avec son récepteur, ils le sont déjà (normalement). C'était le cas de ma FS-i6.

Si par contre vous avez acheté la RC et le récépteur séparément, il est nécessaire de les appairer. Voici comment procéder d'après la doc de la FS-i6:

  1. Connectez le jumper d'appariement sur le port batterie du récepteur
  2. Alimentez le récepteur via la broche d'alim de n'importe quel canal. La LED rouge clignote, indiquant que le récepteur est en mode d'appariement
  3. Maintenez appuyé le bouton d'appariement de la radiocommande et allumez-la
  4. Le processus d'appariement est terminé lorsque la LED du récepteur clignote plus lentement.
    Retirez le jumper, la LED doit se figer
Binding receiver

Votre récepteur est maintenant appairé avec votre radiocommande.

3. Tester la transmission

Nous l'avons vu dans le chapitre 4, les ESC réagissent à un signal de type "servo control", c'est-à-dire un signal utilisé pour contrôler des servo-moteurs. Une impulsion d'une durée de $2000\mu s$ fera tourner le moteur à fond tandis qu'une impulsion de $1000 \mu s$ stoppera totalement le moteur.

Les signaux de sortie du récepteur RF sont également de type "servo control". On peut donc les utiliser directement avec nos ESC, ne serait-ce que pour vérifier que la transmission émetteur/récepteur fonctionne correctement. Ça va également nous permettre de vérifier le numéro de canal de chaque instruction.

Pour ce faire, branchez le connecteur 3 broches d'un ESC sur un canal de sortie du récepteur. Si votre ESC est équipé d'une sortie BEC, le récepteur sera alimenté par l'ESC. Sinon, à vous de l'alimenter avec ce que vous avez sous la main (batterie, alim de labo, ...etc).

Allumez votre radiocommande et alimentez votre récepteur. La FS-i6 doit émettre un bip indiquant qu'elle a bien établi la communication avec son récepteur. De son côté, la diode témoin du récepteur est fixe. Tout est prêt.

Poussez doucement le joystick des gaz (throttle) vers le haut. Le moteur relié au récepteur doit commencer à tourner.

Amusez-vous à vérifier le canal de chaque instruction et notez-vous ça quelque part, ça vous sera utile plus tard.

4. Décoder le message reçu

On a vérifié que notre radiocommande et son récepteur fonctionnent. Mais on ne va pas s'amuser à contrôler chaque moteur indépendamment (je vous laisse imaginer la galère pour effectuer un simple décollage...), c'est le boulot de l'ordinateur de bord.

Il va donc falloir décoder l'information de chaque canal pour la rendre exploitable par l'ordinateur de bord.

4.1 Le signal "servo control"

Nous l'avons vu dans le chapitre sur le calibrage des ESC, ces derniers acceptent en entrée un signal de type servomoteur. Ce signal est une impulsion périodique dont la durée varie entre $1000\mu s$ et $2000\mu s$. La fréquence du signal n'a pas d'importance particulière, seule compte la durée de l'impulsion sur une période.

servo-control signal
Signal de type servo

En l'occurrence, la fréquence du signal en sortie de mon récepteur est de $50Hz$.

Pour de plus amples informations, je vous renvoie au chapitre sur le calibrage des ESC.

4.2 Les interruptions

Pour pouvoir décoder ce signal, nous allons utiliser une fonctionnalité disponible sur la plupart des microcontrôleurs: les interruptions.

Comme son nom l'indique, une interruption met en pause le programme principal pour aller exécuter une autre fonction (qu'on appelle routine d'interruption). Lorsque cette routine d'interruption est terminée, le programme principal reprend là où il s'était arrêté.

interrupt sub routine
Principe d'interruption

On distingue deux types d’interruptions :

  • les interruptions provenant de périphériques externes (reset, bouton poussoir, clavier, changement d’état d’un signal extérieur, communication série ...)
  • les interruptions provenant de périphériques internes (timer, passage par zéro d’un compteur interne, ADC, ..)

Celles qui vont nous intéresser sont les interruptions provenant d'un périphérique externe, ce dernier étant le récepteur FS-i6 dans notre cas.

Une routine d'interruption se doit d'être la plus courte possible : si sa durée d'exécution dépasse la période du signal d'interruption, le programme principal sera continuellement interrompu et donc plus jamais exécuté.

Concrètement, pour utiliser simplement les interruptions de notre Arduino, on utilise la fonction attachInterrupt():


volatile byte state = LOW;

void setup() {
	pinMode(2, INPUT_PULLUP);
	attachInterrupt(digitalPinToInterrupt(2), mySubRoutine, CHANGE);
}

void mySubRoutine() {
	state != state;
}

Du fait qu'elles soient utilisées lors de routines d'interruption, les variables sont suceptibles de changer à n'importe quel moment. Pour l'indiquer au compilateur, on utilise le mot-clé volatile à la déclaration de variable.
De cette manière, le compilateur va charger ces variables en RAM plutôt que dans un registre de stockage.
N'oubliez donc pas de déclarer en volatile toutes les variables globales que vous utilisez dans vos ISR.
Plus d'info ici.

Le problème avec la fonction attachInterrupt() c'est que dans le cas de l'Arduino Uno, elle ne permet de gérer que 2 interruptions, sur les broches 2 et 3 comme le montre ce tableau tiré de la doc officielle:

Board Digital Pins Usable For Interrupts

Uno, Nano, Mini, other 328-based

2, 3

Mega, Mega2560, MegaADK

2, 3, 18, 19, 20, 21

Micro, Leonardo, other 32u4-based

0, 1, 2, 3, 7

Zero

all digital pins, except 4

MKR1000 Rev.1

0, 1, 4, 5, 6, 7, 8, 9, A1, A2

Due

all digital pins

101

all digital pins (Only pins 2, 5, 7, 8, 10, 11, 12, 13 work with CHANGE)

Or, nous avons pas moins de 4 voies à décoder, 2 interruptions ne suffisent pas.

Il est cependant possible d'utiliser plus que 2 interruptions, mais ça nécessite d'aller fouiller un peu dans la datasheet de l'ATmega328P, ce que nous allons faire !

4.3 Les registres d'interruption

On se dit qu'on va utiliser les broches 8, 9, 10 et 11 pour décoder les 4 voies du récepteur.

En regardant le pin mapping de l'ATmega328P, on constate que les interruptions liées à ces broches sont les suivantes:

  • Broche #8: PCINT0
  • Broche #9: PCINT1
  • Broche #10: PCINT2
  • Broche #11: PCINT3
Pin mapping ATmega
Pin mapping de l'ATmega328P

On connaît maintenant leur petit nom. Reste à dire au microcontrôleur qu'on souhaite les utiliser.

Pour cela, direction la datasheet de l'ATmega, section 17 sur les "External interrupts".
On constate en 17.2.8 que ces interruptions sont gérées par le registre PCMSK0 (Pin Change Mask Register 0):

PCMSK0
Each PCINT[7:0] bit selects whether pin change interrupt is enabled on the corresponding I/O pin. If
PCINT[7:0] is set and the PCIE0 bit in PCICR is set, pin change interrupt is enabled on the corresponding
I/O pin. If PCINT[7:0] is cleared, pin change interrupt on the corresponding I/O pin is disabled.

En lisant ça, on comprend que nous avons deux choses à faire:

  • Activer le bit PCIE0 dans le registre PCICR
  • Activer les interruptions qui nous intéressent dans le registre PCMSK0

Direction la section 17.2.4 qui explique le fonctionnement du registre PCICR (Pin Change Interrupt Register):

PCICR
Bit 0 – PCIE0: Pin Change Interrupt Enable 0
When the PCIE0 bit is set and the I-bit in the Status Register (SREG) is set, pin change interrupt 0 is
enabled. Any change on any enabled PCINT[7:0] pin will cause an interrupt. The corresponding interrupt
of Pin Change Interrupt Request is executed from the PCI0 Interrupt Vector. PCINT[7:0] pins are enabled
individually by the PCMSK0 Register.

Ok, mais comment on fait tout ça en code Arduino ?

Comme ceci:


// Activation du bit PCIE0 dans le registrer PCICR
PCICR  |= (1 << PCIE0);

// Activation des interruptions que l'on souhaite utiliser
PCMSK0 |= (1 << PCINT0); // Activation de l'interruption PCTINT0 (broche #8)
PCMSK0 |= (1 << PCINT1); // Activation de l'interruption PCTINT1 (broche #9)
PCMSK0 |= (1 << PCINT2); // Activation de l'interruption PCTINT2 (broche #10)
PCMSK0 |= (1 << PCINT3); // Activation de l'interruption PCTINT3 (broche #11)

Si vous n'êtes pas à l'aise avec les opérateurs binaires, je vous invite à lire ce tutoriel.

Et voilà ! Chaque changement d'état des broches 8, 9, 10, 11 déclenchera une interruption.

Je croyais qu'une interruption déclenchait une autre fonction. Elle est où là l'autre fonction ?

Pour déclarer une routine d'interruption on utilise l'instruction ISR() (Interrupt Sub Routine):


void setup() {	
    PCICR  |= (1 << PCIE0);
    PCMSK0 |= (1 << PCINT0);
    PCMSK0 |= (1 << PCINT1);
    PCMSK0 |= (1 << PCINT2);
    PCMSK0 |= (1 << PCINT3);
}

ISR(PCINT0_vect) {
	// Notre code ici
}

On peut maintenant faire ce qu'on veut dans la routine d'interruption, en gardant à l'esprit que son temps d'exécution doit être le plus court possible.

4.4 La manipulation de ports

La manipulation de ports permet un contrôle bas-niveau des E/S du microcontrôleur sur une carte Arduino.
L'ATmega168 en possède 3:

  • B (broches 8 à 13)
  • C (entrées analogiques)
  • D (broches 0 à 7)
arduino ports
Ports de l'Arduino Uno

Chaque port est contrôlé par 3 registres:

  • DDR: Data Direction Register - définit si une broche est une entrée (INPUT) ou une sortie (OUTPUT)
  • PORT: définit l'état d'une sortie, haut (HIGH) ou bas (LOW)
  • PIN: lit l'état d'une broche définie en tant qu'en entrée

Chaque bit de chaque registre correspond à une broche bien précise. Par exemple, le bit de poid faible de DDRB, PORTB, PINB correspond à la broche PB0 (broche #8).

Voici quelques exemples d'utilisation:


DDRB |= B00000001; // Défini la broche #8 comme sortie
PORTB |= B00000001; // Applique l'état HAUT à la broche #8
PORTB &= B11111110; // Applique l'état BAS à la broche #8
PINB & B00000010; // Lit l'état de la broche #9

En général, manipuler les ports à la main n'est pas une bonne pratique. Pourquoi ? Voici quelques raisons:

  • Le code est bien plus difficile à comprendre et à maintenir
  • Le code est moins portable. En utilisant digitalRead() et digitalWrite(), il est plus simple d'écrire du code qui tournera sur tous les microcontroleurs Atmel alors que les registres et ports peuvent être différents d'un microcontrôleur à l'autre
  • On a vite fait de se tromper d'un bit. Notez que l'instruction DDRD = B11111110; laisse le pin 0 en tant qu'entrée. La broche 0 correspond à la ligne de réception du port série (Rx). Il est donc très facile de planter son port série en changeant la broche 0 en sortie ! Avouez que ça deviendrait vite problématique de ne plus pouvoir envoyer d'instruction à l'Arduino via le port série, non ?

Ok, alors pourquoi on voudrait bidouiller les ports à la main dans ce cas ?

Voici quelques contre-arguments:

  • On peut vouloir changer l'état de sorties très rapidement (en quelques µs). Si on regarde le code source dans /targets/arduino/wiring.c, on peut voir que digitalRead() et digitlWrite() font plus d'une dizaine de lignes de code, qui seront compilées en quelques instructions-machines. Chaque instruction requière au minimum un cycle d'horloge à 16MHz ce qui peut vite être trop long dans le cas d'une application nécessitant de la réactivité. Manipuler les ports directement permet d'obtenir le même résultat en économisant de précieux cycles d'horloge
  • On peut vouloir changer l'état de plusieurs sorties exactement au même moment. Appeler digitalWrite(10, HIGH); suivi de digitalWrite(11, HIGH); va appliquer l'état HAUT à la broche 10 quelques microsecondes avant la broche 11, ce qui pourrait perturber des circuits externes qui seraient branchés dessus. Avec la manipulation de port, on peut appliquer les niveaux de sorties à plusieurs broches exactement au même moment, par ex. en utilisant PORTB |= B11000000;
  • Si votre code devient trop conséquent pour la mémoire du microcontrôleur, utiliser ces astuces vous permet de l'alléger

Dans le cas de notre décodage et plus généralement du drone, on se doit d'avoir une routine d'interruption la plus courte possible. La manipulation de port nous permet de répondre à cette contrainte.

C'est vraiment efficace de manipuler les ports ?

Sans aucun doute ! Regardez plutôt ces relévés d'oscilloscope:

pulse 1
Impulsion créée avec digitalWrite()
pulse 2
Impulsion créée par manipulation de port

Le résultat est sans appel. Quelques $3400 ns$ avec digitalWrite() contre $150 ns$ en manipulant les ports, soit 22 fois plus rapide !

Je vous invite à garder sous le coude la documentation officielle.

Puisqu'on parle optimisation et performance, je vous annonce tout net que déporter des portions de code dans des fonctions perso n'affecte en rien les temps d'exécution. Aussi, pas d'excuse pour coder comme un goret et tout foutre dans la fonction loop(). On pense lisibilité et on organise son code 😉

4.5 Implémentation

4.5.1 Algorithme

Nous savons maintenant que c'est la durée de l'impulsion du signal servo qui représente l'information. Le but de notre code est de mesurer la durée des impulsions de chaque canal. On sait que chacune a une durée comprise entre $1000\mu s$ et $2000\mu s$.

Pour ce faire, nous allons déclencher un timer sur chaque front montant et l'arrêter au prochain front descendant. De cette manière, on mesure bien la durée de l'impulsion:

algo
Algorithme

Le passage d'un état BAS à un état HAUT est appelé front montant.
Le passage d'un état HAUT à un état BAS est appelé front descendant.

Pour démarrer/stopper le timer, nous allons utiliser les interruptions qui seront déclenchées sur chaque front (montant ou descendant). Et pour que notre routine d'interruption soit la plus courte possible, nous allons directement manipuler les ports d'E/S de l'Arduino.

4.5.2 Exemple à une interruption

Pour que vous compreniez bien, voici un exemple avec une seule voie,  la broche 8 par exemple.

On se trouve dans la routine d'interruption ce qui signifie qu'il y a eu un changement d'état d'une broche. On va donc commencer par déterminer s'il sagit d'un front montant ou descendant:

  • SI état = HAUT et que état précédent = BAS, ALORS il s'agit d'un front montant
  • SINON, SI état = BAS et que état précédent = HAUT, ALORS il s'agit d'un front descendant

Voici comment traduire ça en langage Arduino:


if (PINB & B00000001) {    
    if (previous_state == LOW) {  
       // Il s'agit d'un front montant
    }
} else if(previous_state == HIGH) { 
    // Il s'agit d'un front descendant
}

Dans tous les cas, il faut sauvegarder l'état actuel dans previous_state pour la prochaine interruption.
Ensuite, dans le cas d'un front montant, on démarre un timer. Dans le cas d'un front descendant, on le stoppe et on sauvegarde la durée mesurée:


current_time = micros();

if (PINB & B00000001) {                                        
    if (previous_state == LOW) {                       
        previous_state = HIGH; // On sauvegarde l'état actuel
        timer = current_time;  // Et on démarre le timer            
    }
} else if(previous_state == HIGH) {                     
    previous_state = LOW;                  // On sauvegarde l'état actuel                        
    pulse_duration = current_time - timer; // On calcul et on sauvegarde la durée mesurée
}

Plutôt que d'appeler micros() plusieurs fois, ce qui est gourmand en cycles d'horloge, on l'appelle une fois et on stocke le résultat dans une variable.

C'est tout ! C'est exactement le même principe pour les autres broches.

4.5.3 Solution finale

Avec tout ce que nous venons de voir, vous êtes maintenant capables d'implémenter l'algo complet. Voici la solution que je vous propose:


#define CHANNEL1 0
#define CHANNEL2 1
#define CHANNEL3 2
#define CHANNEL4 3

#define YAW      0
#define PITCH    1
#define ROLL     2
#define THROTTLE 3

volatile unsigned long current_time;
volatile unsigned long timer[4];

// Previous state of each channel (HIGH or LOW)
volatile byte previous_state[4];

// Duration of the pulse on each channel in µs (must be within 1000µs & 2000µs)
volatile unsigned int pulse_duration[4] = {1500, 1500, 1000, 1500};

int mode_mapping[4];

/**
 * Setup routine.
 *
 * @return void
 */
void setup()
{
    Serial.begin(9600);

    // Customize mapping of controls: set here which command is on wich channel.
    mode_mapping[YAW]      = CHANNEL4;
    mode_mapping[PITCH]    = CHANNEL2;
    mode_mapping[ROLL]     = CHANNEL1;
    mode_mapping[THROTTLE] = CHANNEL3;

    PCICR  |= (1 << PCIE0);  //Set PCIE0 to enable PCMSK0 scan.
    PCMSK0 |= (1 << PCINT0); //Set PCINT0 (digital input 8) to trigger an interrupt on state change.
    PCMSK0 |= (1 << PCINT1); //Set PCINT1 (digital input 9)to trigger an interrupt on state change.
    PCMSK0 |= (1 << PCINT2); //Set PCINT2 (digital input 10)to trigger an interrupt on state change.
    PCMSK0 |= (1 << PCINT3); //Set PCINT3 (digital input 11)to trigger an interrupt on state change.
}

/**
 * Main loop routine.
 *
 * @return void
 */
void loop()
{
    // Simply dump received data.
    dumpChannels();
}

/**
 * Print received instruction for each channel in the serial port for debug.
 *
 * @return void
 */
void dumpChannels()
{
    for (int i = CHANNEL1; i <= CHANNEL4; i++) {
        Serial.print("Channel ");
        Serial.print(i+1);
        Serial.print(": ");
        Serial.print(pulse_duration[i]);
        Serial.print("\t");
    }
    Serial.print("\n");
}

/**
 * This Interrupt Sub Routine is called each time input 8, 9, 10 or 11 changed state.
 * Read the receiver signals in order to get flight instructions.
 *
 * This routine must be as fast as possible to prevent main program to be messed up.
 * The trick here is to use port registers to read pin state.
 * Doing (PINB & B00000001) is the same as digitalRead(8) with the advantage of using less CPU loops.
 * It is less conveniant but more efficient, which is the most important here.
 *
 * @see https://www.arduino.cc/en/Reference/PortManipulation
 */
ISR(PCINT0_vect)
{
    current_time = micros();

    // Channel 1 -------------------------------------------------
    if (PINB & B00000001) {                                        // Is input 8 high ?
        if (previous_state[CHANNEL1] == LOW) {                     // Input 8 changed from 0 to 1 (rising edge)
            previous_state[CHANNEL1] = HIGH;                       // Save current state
            timer[CHANNEL1]          = current_time;               // Start timer
        }
    } else if(previous_state[CHANNEL1] == HIGH) {                  // Input 8 changed from 1 to 0 (falling edge)
        previous_state[CHANNEL1] = LOW;                            // Save current state
        pulse_duration[CHANNEL1] = current_time - timer[CHANNEL1]; // Stop timer & calculate pulse duration
    }

    // Channel 2 -------------------------------------------------
    if (PINB & B00000010) {                                        // Is input 9 high ?
        if (previous_state[CHANNEL2] == LOW) {                     // Input 9 changed from 0 to 1 (rising edge)
            previous_state[CHANNEL2] = HIGH;                       // Save current state
            timer[CHANNEL2]          = current_time;               // Start timer
        }
    } else if(previous_state[CHANNEL2] == HIGH) {                  // Input 9 changed from 1 to 0 (falling edge)
        previous_state[CHANNEL2] = LOW;                            // Save current state
        pulse_duration[CHANNEL2] = current_time - timer[CHANNEL2]; // Stop timer & calculate pulse duration
    }

    // Channel 3 -------------------------------------------------
    if (PINB & B00000100) {                                        // Is input 10 high ?
        if (previous_state[CHANNEL3] == LOW) {                     // Input 10 changed from 0 to 1 (rising edge)
            previous_state[CHANNEL3] = HIGH;                       // Save current state
            timer[CHANNEL3]          = current_time;               // Start timer
        }
    } else if(previous_state[CHANNEL3] == HIGH) {                  // Input 10 changed from 1 to 0 (falling edge)
        previous_state[CHANNEL3] = LOW;                            // Save current state
        pulse_duration[CHANNEL3] = current_time - timer[CHANNEL3]; // Stop timer & calculate pulse duration
    }

    // Channel 4 -------------------------------------------------
    if (PINB & B00001000) {                                        // Is input 11 high ?
        if (previous_state[CHANNEL4] == LOW) {                     // Input 11 changed from 0 to 1 (rising edge)
            previous_state[CHANNEL4] = HIGH;                       // Save current state
            timer[CHANNEL4]          = current_time;               // Start timer
        }
    } else if(previous_state[CHANNEL4] == HIGH) {                  // Input 11 changed from 1 to 0 (falling edge)
        previous_state[CHANNEL4] = LOW;                            // Save current state
        pulse_duration[CHANNEL4] = current_time - timer[CHANNEL4]; // Stop timer & calculate pulse duration
    }
}

Comme chacun a ses préférences en ce qui concerne le mode de sa radiocommande, j'ai prévu dans le code un mapping qui vous permet d'associer chaque commande au canal de votre choix.
À vous de modifier en conséquence cette portion du setup():


    mode_mapping[YAW]      = CHANNEL4;
    mode_mapping[PITCH]    = CHANNEL2;
    mode_mapping[ROLL]     = CHANNEL1;
    mode_mapping[THROTTLE] = CHANNEL3;

Le mapping par défaut correspond au mode 2.

5. Câblage et test

Il est maintenant temps de tester notre code. Commencez par relier la broche "signal" de chaque canal à votre Arduino en respectant le mapping suivant:

  • Canal 1 →Broche 8
  • Canal 2 →Broche 9
  • Canal 3 →Broche 10
  • Canal 4 →Broche 11
Cablage
Câblage de l'arduino avec le récepteur

N'oubliez pas la masse commune

Branchez l'Arduino à votre PC, alimentez récepteur & radiocommande et ouvrez un terminal.

Vous devriez voir défiler les durées d'impulsion en $ms$ de chaque canal. Bougez les sticks de votre radiocommande et vérifiez que les valeurs qui défilent correspondent.

Terminal
Durées d'impulsion des 4 voies

6. Conclusion

Nous avons vu beaucoup de concepts assez poussés avec notre Arduino qui nous ont permis de décoder un signal de type servomoteur.

Gardez à l'esprit que la manipulation de ports est beaucoup moins coûteuse en cycles d'horloge que l'utilisation des fonctions génériques telles que digitalWrite().

Il s'agit d'un chapitre assez complexe alors prenez votre temps pour tout assimiler et relisez-le plusieurs fois si nécessaire.

La partie RF de notre drone est maintenant fonctionnelle. Dans le prochain article nous verrons comment mettre en place un Inertial Measurment Unit avec le capteur MPU6050.

Sources: [1] [2] [3] [4]

Vos réactions (0) :

  1. Sois le/la premier(e) à commenter cet article !
Tu as besoin d'aide ? Utilise le Forum plutôt que les commentaires.

Un commentaire ?

* Champs obligatoires
Utilisation des données

Afin d'améliorer ton expérience utilisateur, nous utilisons des cookies 🍪