Generación de protocolo SUMD y comprobación en FC (Controladora de vuelo de drones)
Las FC (controladora de vuelo) se utilizan para gestionar el funcionamiento de drones, aviones, barcos, etc. Disponen de acelerometro, giroscopo, barometro, etc, etc. En el caso de los drones y aviones, permiten la estabilización del modelo por medio de la IMU (acelerometro y giroscopo).
Los datos para que nuestro equipo haga lo que queremos, llegan a la FC desde el receptor.
Habitualmente la comunicacion entre el receptor y la FC es mediante UART a 115.200 bps 8N1 y existen multiples protocolos. Uno de ellos es el SUMD.
Asi se ve una trama del protocolo SUMD
El primer byte indica el fabricante
El segundo solo tiene 2 valores validos 0x01 (Rx ok) y 0x81 (indica a la FC que se active Fail Safe), cualquier otro valor invalida la trama.
El tercero indica el numero de canales que estamos enviando. El máximo son 32.
Los siguientes y por parejas indican el valor de cada canal.
El valor a enviar para cada canal se calcula multiplicando por 8 el valor en microsegundos que queremos enviar.
Y los 2 últimos son el calculo del CRC de la trama. Si el CRC es incorrecto, la trama será desechada.
Dentro de la comunicación deberemos enviar datos como el valor del acelerador, si queremos avanzar o retroceder, ir a la izquierda o a la derecha, girar sobre nosotros mismos, y multiples posibilidades mas.
Ejemplo de una FC
Habitualmente se conecta un receptor a la entrada de la FC para el control en remoto del aeromodelo (drone, avion, etc.)
Vista del montaje de pruebas:
He simulado, mediante un ESP32 el protocolo con el siguiente soft:
/*
gestion de la FC del drone
- envio de datos con protocolo SUMD 16 CANALES
*/
#include "CRC16.h"
#include "CRC.h"
unsigned long seven_milis;
char str[40] = {0xa8,0x01,0x10, //16x2 + 3 =35 27
0x22,0x60, //1100
0x2e,0xe0, //1500
0x3b,0x60, //1900
0x41,0xa0, //2100
0x1f,0x40, //1000
0x2e,0xe0, //1500
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2E,0xE0};
CRC16 crc;
void calcula_crc()
{
crc.setPolynome(0x1021);
crc.add((uint8_t*)str, 35);
str[35] = crc.calc() >> 8;
str[36] = crc.calc() & 0xFF;
crc.reset();
for (int alfa = 0; alfa <= 36; alfa++) // envio el array completo: datos + crc
{
Serial1.print(str[alfa]);
}
}
void setup()
{
// Initialize Serial Monitor
Serial.begin(115200);
Serial1.begin(115200, SERIAL_8N1,26,25,true); // SUMD FC
Serial1.write("\n");
}
void loop()
{
// rutina para el envio de protocolo SUMD a la FC del drone
if(millis()-seven_milis>10)
{
seven_milis=millis();
calcula_crc(); // obtiene el CRC,lo añade al final del array y lo envia
}
}
En la FC he cargado y configurado el soft Betafligh para comprobar el correcto funcionamiento del simulador de protocolo SUMD
El soft del ESP32 esta simplificado al máximo para hacer la comprobación de su correcto funcionamiento.
El envio de tramas SUMD deber ser constante y en intervalos no superiores a 10 milisegundos, de otra forma, la FC entiende que se ha cortado la comunicación con el receptor y activa el modo SAFE, procediendo a aterrizar o volviendo al punto de despeque en el caso de llevar GPS y estar asi configurado.
Ejemplo de tren de tramas no correcto:
Y su efecto en la FC del drone:
Por tanto, el tren de tramas debe presentar un aspecto mas o menos asi (la separacion entre ellas no llega a los 10 ms):
Puede ocurrir que al cargar con más codigo al ESP32, "descuide" el envio de tramas SUMD al estar ejecutando otras rutinas y el envio de tramas SUMD se vea afectado, aumentando el tiempo entre ellas a mas de 10 ms.
Dado que ESP32 dispone de 2 nucleos, esto se soluciona cargando el trabajo del envio de las tramas SUMD al nucleo ocioso.
Mientras que el nucleo titular se encarga del resto:
/*
gestion de la FC del drone
- envio de datos con protocolo SUMD 16 CANALES
*/
#include "CRC16.h"
#include "CRC.h"
unsigned long seven_milis;
char str[40] = {0xa8,0x01,0x10, //16x2 + 3 =35 27
0x22,0x60, //1100
0x2e,0xe0, //1500
0x3b,0x60, //1900
0x41,0xa0, //2100
0x1f,0x40, //1000
0x2e,0xe0, //1500
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2e,0xe0,
0x2E,0xE0};
CRC16 crc;
void calcula_crc()
{
crc.setPolynome(0x1021);
crc.add((uint8_t*)str, 35);
str[35] = crc.calc() >> 8;
str[36] = crc.calc() & 0xFF;
crc.reset();
for (int alfa = 0; alfa <= 36; alfa++) // envio el array completo: datos + crc
{
Serial1.print(str[alfa]);
}
}
//**************************
TaskHandle_t Task1;
//**************************
//************************************************************************************************
void Task1code( void * pvParameter)
{
while(1)
{
// rutina para el envio de protocolo SUMD a la FC del drone
if(millis()-seven_milis>10)
{
calcula_crc(); // obtiene el CRC,lo añade al final del array y lo envia
seven_milis=millis();
}
}
}
void setup()
{
//*************************************************************************************************
xTaskCreatePinnedToCore(
Task1code, /* Function to implement the task */
"protocolo SUMD", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
&Task1, /* Task handle. */
0); /* Core where the task should run */
//*************************************************************************************************
// Initialize Serial Monitor
Serial.begin(115200);
Serial1.begin(115200, SERIAL_8N1,26,25,true); // SUMD FC invertido
Serial1.write("\n");
}
void loop()
{
// ya tienes recursos de procesador para que hagas otras cosas
}
Utilizo platformio para programar y la configuración del archivo platformio.ini es la siguiente:
[env:esp32doit-devkit-v1]
platform = espressif32@5.1.0
board = esp32doit-devkit-v1
framework = arduino
monitor_speed = 115200
monitor_port = COM5
upload_speed = 921600
upload_port = COM5
lib_deps =
robtillaart/CRC@^1.0.3
Para el calculo del CRC utilizo la libreria robtillaart/CRC@^1.0.3 que te puedes descargar facilmente desde platformio home.
SEÑAL INVERTIDA
A veces puede ocurrir que una controladora necesita que la señal SUMD le llegue invertida.
Esto sería una señal en modo normal:
Y esta sería una señal invertida:
La diferencia es el punto de partida de la señal. En el primer caso la señal en reposo esta a 1 lógico y en el segundo caso esta a 0 lógico.
Esto se configura asi en el ESP32:
Serial1.begin(115200, SERIAL_8N1,26,25,true); // SUMD FC señal invertida
Serial1.begin(115200, SERIAL_8N1,26,25); // SUMD FC señal normal
De esta manera evitamos añadir transistores al circuito para invertir la señal.