Télémétrie de vol d'un drone

Il y a quelques années, j’ai acheté un petit drone pas cher : Tello. On peut le piloter avec son smartphone au travers d’une communication UDP sur du WiFi. J’ai décidé de le “hacker” et de regarder les données de télémétrie de vol qu’il émet.

Prenant mon courage à deux bras, j’ai développé une petite librairie NodeJS pour pouvoir le piloter depuis un bout de code, récupérer les données de vol en temps réel et stream la vidéo de la caméra. Après plusieurs heure et litres de café, j’y suis parvenu, vous la trouverez là. J’ai des enfants et le but principal était de les faire développer en Javascript en s’amusant. En fait, mes enfant n’y ont prêté aucun intérêt et le seul enfant qui joue avec, c’est moi.

Si vous possédez ce drone, vous pouvez bien sûr utiliser cette lib. Ce petit drone communique en UDP. Il expose un serveur pour recevoir des commandes et a un client pour transmettre ses données de vol à un serveur UDP tiers.

Les données de télémétrie disponibles sont :

Name

Unit

Description

h

cm

altitude

baro

cm

baromètre

tof

cm

distance au sol

templ

°C

temperature mini de la batterie

temph

°C

temperature maxi de la batterie

pitch

°

tangage

roll

°

roulis

yaw

°

lacet

agx

0.001g

accéleration x

agy

0.001g

accéleration y

agz

0.001g

accelération z

vgx

cm/s

vitesse x

vgy

cm/s

vitesse y

vgz

cm/s

vitesse z

bat

%

batterie

données de télémétrie de vol

https://fr.wikipedia.org/wiki/Pilotage_d%27un_avion

Warp 10

Warp 10 est une plateforme formidable, à la fois une base de données de séries temporelles (TSDB), un moteur d’analyse avec son langage de script (on verra plus tard) mais surtout une plateforme avec plein de plugins dont un permettant d’instancier un serveur UDP.

Plugin UDP

Je pars du postulat que vous avez installé Warp 10 dans /opt/warp10 (pas dans une image docker hein ;o) ), on va activer le plugin UDP. Dans /opt/warp10/etc/conf.d/80-udp-plugin.conf :

// activate the UDP plugin
warp10.plugin.udp = io.warp10.plugins.udp.UDPWarp10Plugin
udp.dir = ${standalone.home}/udp
udp.period = 5000

Il serait bien d’activer l’extension Debug :

warpscript.extension.debug = io.warp10.script.ext.debug.DebugWarpScriptExtension

Ensuite, il faut créer un répertoire /opt/warp10/udp et y placer un fichier tello.mc2 :

'Loading Tello telemetry' LOGMSG
{
  'mode' 'server'
  'host' '0.0.0.0'
  'port' 8890
  'timeout' 5000
  'macro' <% 
      'datagramLIST' STORE
    <% $datagramLIST ISNULL %>
    <%
      'timeout reached, nothing received' LOGMSG
    %>
    <%
      $datagramLIST
      <%
        'datagram' STORE
        $datagram 2 GET 'utf-8' BYTES-> 'dataFrame' STORE
        $dataFrame LOGMSG
        'Your write token' 'token' STORE // Insert your write token here
        $dataFrame ';' SPLIT <%
        // For each data in the dataframe 
        TRIM 'data' STORE
        <% $data '' != %> // manage the trailing ';'
        <% 
            // extract key and value
            $data ':' SPLIT LIST-> DROP [ 'key' 'value' ] STORE
            NEWGTS 'tello.telemetry.' $key + RENAME // create a new GTS
            NOW NaN NaN NaN $value TODOUBLE ADDVALUE // Add the new datapoint
            $token UPDATE
        %> IFT
        %> FOREACH
      %> FOREACH
    %> IFTE
  %>
}

Script de pilotage

Ensuite vous pouvez redémarer Warp 10. Connectez le drone via le WiFi et utilisez ce petit javascript pour le piloter :

import {Tello} from "@giwisoft/ryze-tello-sdk";
const tello = new Tello();
(async () => {
  // Start the engine and play "Ride Of The Valkyries"
  await tello.start();
  await tello.takeoff();
  // Go up
  await tello.up(50);
  // Perform a forward flip
  await tello.flip('f');
  // Go forward
  await tello.forward(50);
  await tello.right(20);
  // Go backward
  await tello.backward(100);
  await tello.rotateCW(360);
  // Finally land
  await tello.land();
  // And then shut down the engine
})().then(() => tello.stop());

Vous pouvez également streamer la vidéo via mplayer :

await tello.startStream();
[...]
await tello.stopStream();

Balancer la télémétrie en direct sur Warp 10 :

await tello.startTelemetry({
  withWarp10: true,
  warp10Params: {
    url: 'http://localhost:8080/api/v0/exec',
    writeToken: 'Token d\'écriture'
  }
});

Balancer la télémétrie en direct sur du MQTT

await tello.startTelemetry({
  withMqtt: true,
  mqttParams: {
    url: 'http://localhost:1883',
    clientId: 'tello'
  }
});

Voici un petit serveur MQTT en JavaScript pour tester :

const aedes = require('aedes')();
const server = require('net').createServer(aedes.handle);
server.listen(1883, () => console.log('server started and listening'));

Et un client pour afficher les traces :

const mqtt = require('mqtt');
const client = mqtt.connect('mqtt://127.0.0.1:1883', { 'clientId': 'PC' });
let connected = false;
client.on('connect', () => {
    console.log("connected")
    client.subscribe('ryze.tello');
    connected = true;
});
client.on('message', (topic, message) => {
    if (topic === 'ryze.tello') {
        console.log(message.toString());
    }
});

Mais bon, le plus fun c’est quand même d’utiliser la com UDP directement avec Warp 10.

Normalement, dès que le drone démarre ses moteurs, les données sont transmises à Warp 10 et en faisant tail -f /opt/warp10/logs/warp10.log, vous devriez voir :

[1616403713307] pitch:0;roll:0;yaw:-156;vgx:0;vgy:0;vgz:0;templ:79;temph:81;tof:10;h:0;bat:82;baro:-99.61;time:31;agx:1.00;agy:-10.00;agz:-998.00;[1616403713409] pitch:0;roll:0;yaw:-156;vgx:0;vgy:0;vgz:0;templ:79;temph:81;tof:10;h:0;bat:82;baro:-99.83;time:31;agx:1.00;agy:-10.00;agz:-1002.00;[1616403713511] pitch:0;roll:0;yaw:-156;vgx:0;vgy:0;vgz:0;templ:79;temph:81;tof:10;h:0;bat:82;baro:-99.87;time:31;agx:1.00;agy:-11.00;agz:-1003.00;

Première analyse de la télémétrie

Et pour visualiser les données, allez sur WarpStudio et affichons la distance au sol (par exemple) :

[ 2021 03 22 10 18 ] TSELEMENTS-> 'start' STORE
[ 2021 03 22 10 15 ] TSELEMENTS-> 'end' STORE
[
  "your read token"
  'tello.telemetry.tof' {}
  $start ISO8601
  $end ISO8601
] FETCH 500 LTTB

Le flip

Et hop, je vous laisse vous amuser à explorer les données.

Aller plus haut, aller plus hauuuuuauuut

Après avoir niqué votre journée en vous ayant collé une chanson dans la tête, je vous encourage à aller lire l’article original qui vous permettra de faire de la détection d’anomalie ainsi que réaliser un dashboard.

Article original

github.com/Giwi/ryze-tello-sdk