Il firmware APM:Copter è una delle principali soluzioni open-source per il pilotaggio remoto di droni. Vedremo come è possibile eseguire codice personalizzato all’interno del sistema (che nel mio caso è installato su una board PixHawk v2), ed inviare dati a terra mediante il protocollo MAVLink.

Il codice di APM:Copter e i submodules

Per prima cosa bisogna clonare il repository da GitHub (link).

Purtroppo ho riscontrato dei problemi con i submodules del progetto, che facevano riferimento ai vecchi URL git:// non più supportati da GitHub. Ho risolto in questo modo:

1. Aprire /ardupilot/.gitmodules e Sostituire tutti i git:// con https://
2. Fare lo stesso in /ardupilot/.git/CONFIG
3. Eseguire git submodule update --init nella directory /ardupilot
4. Eseguire i passi 1-3 per ciascun submodule che contenga un file .gitmodules

Per compilare/installare il firmware fate riferimento a questa sezione della documentazione di ArduPilot.

Gli user hooks

APM:Copter è un esempio di sistema real-time: contiene uno scheduler che pianifica l’esecuzione delle diverse funzionalità del sistema in base alla frequenza desiderata dall’utente (es. 10 Hz) e al tempo previsto per l’esecuzione stessa. In questo modo si garantisce che il drone sia sempre in equilibrio e reattivo agli input dell’utente.

Gli sviluppatori hanno inoltre realizzato un sistema che ci consentirà di eseguire il nostro codice senza dover configurare a mano lo scheduler.

• Il file UserVariables.h serve a definire e inizializzare le variabili;
• UserCode.cpp contiene una funzione userhook_init() che viene eseguita solo una volta all’avvio del sistema, e cinque “loops” con frequenze decrescenti (100, 50, 25, 10, 1 Hz).
• APM_Config.h è un insieme di direttive che permettono la personalizzazione del sistema. Andrà modificato per abilitare i loop appena menzionati.

Impariamo a contare

Realizzeremo una variabile-contatore che incrementa il proprio valore di 1 con una frequenza nominale di 10 Hz.

Lo so che quello del contatore è un esempio trito e ritrito, ma almeno è chiaro e ci permette di capire un sacco di cose sul sistema. Quindi in marcia.

UserVariables.h

Inseriamo la dichiarazione

unsigned int count = 0;

UserCode.cpp

Modifichiamo userhook_MediumLoop() come segue:

#ifdef USERHOOK_MEDIUMLOOP
void Copter::userhook_MediumLoop()
{
    if (ap.initialised)
    {
        count = ( count + 1 ) % 32000; // Un valore a caso per riazzerare count
    }
}
#endif

APM_Config.h

Cerchiamo e decommentiamo queste direttive:

#define USERHOOK_VARIABLES "UserVariables.h"
#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();

Invio dei dati alla stazione di terra

Se siete impazienti come me potreste anche ricompilare e reinstallare il firmware, ma ovviamente non avrete modo di accedere al fantomatico count.

Vediamo allora come fa il drone a comunicare con la GCS (Ground control station), che nel nostro caso è Mission Planner.

MAVLink

Il protocollo MAVLink (faccio riferimento alla versione 1.0) si basa su frame così composti:

L’header a sua volta è composto da 6 campi, ciascuno di un byte:

Il Message ID è fondamentale per consentire al destinatario di decifrare il payload.

\libraries\GCS_MAVLink\GCS.h

aggiungiamo un valore MSG_CUSTOM_COUNT al penultimo posto dell’enum ap_message:

enum ap_message {
    MSG_HEARTBEAT,
    …
    MSG_POSITION_TARGET_GLOBAL_INT,
    MSG_CUSTOM_COUNT,
    MSG_RETRY_DEFERRED // this must be last
};

Copter.h

Aggiungiamo la definizione di questo metodo alla classe omonima:

void send_count(mavlink_channel_t chan);

GCS_Mavlink.cpp

Questo file si occupa di gestire la comunicazione con la GCS e quindi qui si concentrerà la maggior parte del lavoro.

Implementiamo la funzione appena definita:

void NOINLINE Copter::send_count(mavlink_channel_t chan)
{
    mavlink_msg_named_value_int_send(
        chan,
        AP_HAL::millis(),    // uptime del sistema in ms (usato come timestamp)
        "count",
        count
    );
}

Modifichiamo la funzione GCS_MAVLINK::try_send_message() aggiungendo una nuova opzione allo switch(id):

switch(id) {
    …
    case MSG_CUSTOM_COUNT:
        copter.send_count(chan);
        break;
    …
}

Infine facciamo sì che data_stream_send() invii il messaggio:

void GCS_MAVLINK::data_stream_send(void)
{
    …
    send_message(MSG_CUSTOM_COUNT);
    …
}

È opportuno posizionare questa istruzione dopo l’ultima occorrenza del controllo

if (copter.gcs_out_of_time) return;

In questo modo il messaggio sarà tra quelli a minor priorità possibile.

Verifica su Mission Planner

Ora possiamo compilare il firmware e installarlo sulla nostra board. Lanciamo Mission Planner. Nella scheda Help deve essere selezionata la casella “Mostra la CONSOLE (Riavvio)”. In caso contrario occorre selezionarla e riavviare Mission Planner.

A questo punto comparirà una console con alcuni messaggi relativi al funzionamento di Mission Planner. Se ora stabiliamo la connessione con il dispositivo vedremo anche qualche messaggio di diagnostica MAVLink:

bps 1663 loss 0 left 108 mem 23,40625
bps 1613 loss 0 left 108 mem 22,7744140625
bps 1613 loss 0 left 108 mem 22,1181640625
bps 1757 loss 0 left 307 mem 23,0859375

Il tipo di messaggio che abbiamo usato (NAMED_VALUE_INT) non viene visualizzato normalmente: dovremo passare alla scheda Config/Tuning, sezione Planner e selezionare la casella “messaggi debug Mavlink” (in basso a sinistra).

La console ora stamperà tutti i messaggi ricevuti, tra cui quelli personalizzati:

FE 12  0  0 6C  1  1     FC mavlink_named_value_int_t time_boot_ms 98727 value 857 name count  sig  Len 26
FE 12  0  0 8A  1  1     FC mavlink_named_value_int_t time_boot_ms 99227 value 862 name count  sig  Len 26
FE 12  0  0 A5  1  1     FC mavlink_named_value_int_t time_boot_ms 99727 value 867 name count  sig  Len 26
FE 12  0  0 C2  1  1     FC mavlink_named_value_int_t time_boot_ms 100227 value 872 name count  sig  Len 26
FE 12  0  0 DD  1  1     FC mavlink_named_value_int_t time_boot_ms 100727 value 877 name count  sig  Len 26

In altre parole, funziona!

Conclusioni

Abbiamo esposto a grandi linee le modalità con cui è possibile eseguire codice personalizzato sulla PixHawk e rendere disponibili dei dati alla stazione di terra. Notiamo che:

• Se avessimo voluto trasferire un numero a virgola mobile, avremmo dovuto usare la funzione mavlink_msg_named_value_float_send() anziché mavlink_msg_named_value_int_send().
• In genere l’invio di semplici coppie chiave-valore andrebbe limitato alla fase di sviluppo/debug di una nuova funzionalità: per procedere nel modo più corretto, si dovrebbe invece definire un tipo di messaggio personalizzato. Così facendo si semplifica il contenuto del frame (solo valori) e si consente al destinatario di riconoscere facilmente i messaggi da elaborare.

Riferimenti

http://qgroundcontrol.org/mavlink/start (Edit 2020-11-09: Sito offline)
http://mavlink.io/en/messages/common