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

axes d'un avion — 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;<br>[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;<br>[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

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