Cómo recibir y stremear nuevos mensajes MAVLink desde PX4 (usando uORB ya existente)

Es una tarea quizás un poco tediosa, especialmente de primeras, pero intentaré contar mi experiencia y los problemas de conceptos (mayormente) con los que me he ido encontrando. Voy a explicar cómo crear un nuevo mensaje para MAVLink, el cual será enviado a nuestro autopilot (en mi caso con PX4). Una vez allí, PX4 debe ser capaz de gestionarlo y, posteriormente, abrir un nuevo stream para poder enviarlo hacia el exterior.

Figura 1. Envío de nuevo mensaje y sus partes involucradas en este caso

En primer lugar, decir que no siempre es necesario crear nuestros propios mensajes personalizados, pero es bueno saber cómo funciona todo este proceso. Siempre que podamos reutilizar cualquier funcionalidad ya implementada, trabajo que nos ahorramos.

Este post se divide en dos partes que juntas hacen posible 1) la recepción de nuevos mensajes desde PX4 y 2) su posterior reenvío (streaming) hacia otro sistema que esté conectado (vía serial en nuestro caso).

Para iniciar el flujo, contamos con una Adeept Uno (clon de la original) que se encarga de generar y enviar los mensajes. Demos por hecho que hemos escrito un pequeño programa para Arduino capaz de empaquetar y enviar el nuevo mensaje que habremos definido. Vamos a enfocarnos en el manejo del mensaje que tiene lugar en PX4.

Figura 2. Así queda todo el conjunto conectado vía serial. No tengáis en cuenta el LED

Crear un nuevo mensaje MAVLink: PRESS_TEMP_SENSOR

Añadiremos los cambios necesarios a PX4 para que sea capaz de reconocer y procesar el nuevo tipo de mensaje que queremos que reciba desde nuestra Uno y no lo descarte. Primero, vamos a definir este nuevo mensaje y sus propiedades.

MAVLink se basa en el uso de dialectos, los cuales se generan en base a archivos xml que contienen las definiciones necesarias para un dialecto determinado. Por ejemplo, los mensajes propios del dialecto common se crean cuando usamos un generador MAVLink (pymavgen) en base al archivo common.xml. Según el dialecto que importes en tu código, tendrás acceso a determinados mensajes.

Todas las partes involucradas en el envío, recepción o procesamiento de un mensaje deben implementar la definición del mismo en alguno de sus dialectos y, consecuentemente, su respectiva cabecera (mavlink_msg_press_temp_sensor.h en nuestro ejemplo).

Por ende, si queremos enviar paquetes desde Arduino hacia PX4 y luego hacia un script en Python (pymavlink), entonces todas esas partes deben contar con la misma definición del mensaje en cuestión. De lo contrario, el proceso fallará en alguno de esos nodos.

Entonces, digamos que la definición del nuevo mensaje que queremos generar va a incluirse en:

• En PX4 y pymavlink -> development.xml
• En Arduino -> common.xml

Desde PX4

• Añadir en "src/modules/mavlink/mavlink/message_definitions/v1.0/development.xml" la nueva definición para el mensaje (el id debe estar libre).

  

  Figura 3. Definición de nuestro mensaje PRESS_TEMP_SENSOR en development.xml en el entorno de PX4

  
  
• La cabecera de nuestro mensaje mavlink_msg_press_temp_sensor.h será autogenerada una vez hagamos una nueva build del firmware de PX4 (la encontraremos en la respectiva carpeta de su build en /mavlink/development/mavlink_msg_press_temp_sensor.h).
• Para hacer una build del firmware de PX4 recuerda usar el comando make px4_fmu-v5_default (por ejemplo).

Figura 4. Cabecera resultante de haber hecho una build del firmware de PX4 con la pertinente definición de nuestro mensaje

Desde MAVLink Arduino

• Copia la cabecera ya generada para el MAVLink de PX4 en la librería de Arduino, dentro de la carpeta common.
• (*) Esta no es, de lejos, la forma correcta de hacerlo, pero mi placa Uno se quejaba de falta de memoria cuando intentaba flashear una versión de MAVLink generada por mi (fuese la versión 1 o la 2). Si no tienes este problema, hazlo de la otra manera.
• Como lo estamos haciendo manualmente, tenemos que actualizar common.h para indicar dónde está el nuevo mensaje. Añade #include "./mavlink_msg_press_temp_sensor.h" al final de la lista.
• (*) Si al verificar el código en Arduino tienes problemas con la definición de la función mavlink_finalize_chan (originalmente definida en mavlink_helpers.h), ve a la cabecera del nuevo mensaje y elimina todos los parámetros llamados “MAVLINK_MSG_ID_PRESS_TEMP_SENSOR_MIN_LEN” de dicha función. Esto es por que la librería MAVLink que he usado desde Arduino y la de PX4 no son la misma versión, y la definición de dicha función es ligeramente distinta.

Desde pymavlink

Por último, ya que tenemos pensado leer el mensaje que PX4 stremeará desde un script de Python, tendremos que actualizar nuestra librería pymavlink para que incluya nuestra nueva definición.

Para ello, si tenemos ya instalado pymavlink de antes, vamos a eliminarla (en caso de que no tengamos nada importante generado de antes que no queramos perder) usando $ pip3 uninstall pymavlink . Luego vamos a clonar el código fuente de MAVLink mediante $ git clone https://github.com/mavlink/mavlink.git .

Una vez clonado, ejecutemos el comando $ git submodule update --init --recursive para descargar el contenido de la librería pymavlink. Luego, dentro de ese directorio, vayamos a /message_definitions y añadamos nuestra definición en el dialecto deseado (del mismo modo que antes), en development.xml.

Ahora toca actualizar pymavlink, para ello lancemos el comando $ sudo python3 setup.py install (desde fuera de /pymavlink). Al terminar, nuestra librería de MAVLink para Python está actualizada con la definición de nuestro nuevo mensaje PRESS_TEMP_SENS.

Por último, tenemos que editar el archivo /pymavlink/mavutil.py para decirle qué dialecto queremos que tome (nos interesa el que contiene la definición de nuestro mensaje). Tendremos que sustituir esta línea donde dice 'ardupilotmega' por 'development' tal y como sigue:

Figura 5. Indicar a mavutil (módulo principal de pymavlink) qué dialecto debe tener en cuenta

Todo este proceso ha sido solamente para crear el mensaje en sí y hacer saber a las instancias MAVLink de cada sistema que ese mensaje existe en un determinado dialecto. Podéis imaginar cuán no recomendable es evitar crear nuestros propios mensajes a toda costa, a no ser que sea totalmente necesario.

Recepción del mensaje en PX4

Para ello tenemos que hacer modificaciones en un archivo específico dentro del módulo de MAVLink de PX4. Ve a “src/modules/mavlink/mavlink_receiver.cpp”, ahí tendremos que añadir una función encargada de gestionar la recepción de un nuevo mensaje.

Figura 6. Gestor del mensaje entrante en PX4

En esta función creamos una nueva instancia del uORB topic sensor_baro, el cual cargaremos con la información que contengan los diferentes campos de nuestro mensaje tras decodificarlo (press_abs, temperature).

Esto de los uORB topics es un poco como la manera de decirle a PX4 que el contenido de un mensaje entrante pertenece a un determinado tópico y que debe informar al sistema de él (publish) y hacerlo accesible para sistemas externos que lo soliciten (subscribe).

Una vez decodificado nuestro mensaje de entrada y preparada una instancia de la estructura de sensor_baro, pasamos a publicar dicha nueva instancia. Esto quiere decir que PX4 habrá recibido, interpretado y publicado la información de dicho mensaje como una nueva instancia de un uORB topic ya existente. (Más y mejor sobre uORB messaging en https://px4.io/px4-uorb-explained-part-1/).

Para comprobar que todo está funcionando correctamente, activemos el envío de mensajes desde el script en Arduino y abramos la consola de PX4 desde nuestro GCS (Q Ground Control por ejemplo). Eso sí, antes tenemos que buildear el firmware y flashearlo a nuestro autopilot, o hacerlo mediante SITL (Software In the Loop).

Una vez dentro, escribamos el comando listener sensor_baro, este nos mostrará todas las instancias activas del uORB topic sensor_baro. Deberíamos ver dos instancias, la instancia 0 (con id=0x3D) será la correspondiente al barómetro interno de nuestro autopilot. La instancia 1 (con id=0x3F) será la correspondiente a los mensajes que estamos generando en base a nuestro propio barómetro desde Arduino.

Figura 7. Las dos instancias disponibles del uORB topic sensor baro. La segunda es la nuestra.

(*) No olvides añadir nuestro case más arriba en el mismo archivo para redirigir el mensaje PRESS_TEMP_SENSOR directamente hacia nuestro handler. Debe tener esta forma, pero personalizado para nuestro mensaje:

Figura 8. Case que actúa como multiplexor para según qué tipo de mensaje ha sido recibido en mavlink_receiver.cpp

Crear stream del mensaje para que sea leído desde el script de Python

Para ello tenemos que crear un archivo dentro de "/src/modules/mavlink/stream" que llamaremos PRESS_TEMP_SENSOR.hpp. Este se encargará de habilitar la suscripción al topic para que podamos recibirlo desde Python en forma de mensaje.

El archivo se encarga de crear un nuevo stream MavlinkStreamPressTempSensor que hereda de la clase MavlinkStream. Así, creamos una suscripción para nuestra instancia de sensor_baro, la cual enviará la información pertinente en forma de mensaje al exterior. Un poco el proceso "inverso" a la publicación.

Figura 9. Creamos el stream que suscribirá nuestra instancia de sensor_baro para que sea enviada al exterior en forma de mensaje

(*) Una vez listo esto, no podemos olvidarnos de añadir un include a nuestro stream en "/src/modules/mavlink/mavlink_messages.cpp", así como de crear una instancia de nuestro MavlinkStreamPressTempSensor del siguiente modo:

Figura 10. Crear instancia de nuestro stream personalizado

(*) También tendremos que establecer un ratio (frecuencia) de stremeo, para ello vayamos a "/src/modules/mavlink/mavlink_main.cpp" y añadamos "configure_stream_local("PRESS_TEMP_SENSOR", 10.0f);" dentro de los case MAVLINK_MODE_NORMAL y MAVLINK_MODE_ONBOARD.

Finalmente, comprueba que estás recibiendo correctamente el mensaje usando un script en python con la librería que previamente modificamos. Bastaría con abrir una conexión al serial deseado, por ejemplo, y guardar el mensaje así: msg = conexion.recv_match(type='PRESS_TEMP_SENSOR', blocking=True) y luego mostrar algo en consola: print(msg.temperature).

¡Espero que haya sido de buen gusto!

Cualquier malentendido o perspectiva errónea descrita anteriormente que sea señalada será abiertamente considerada.