Mejora DMA en Prototipo de Driver Dynamixel con Teensy 3.2

Después de hacer un poco de investigación. decidí configurar mi Prototipo de Driver Dynamixel  con Teensy 3.2 para mi DARwIn-OP de forma de eliminar el código de retransmisión del loop:

void loop() 
{
  if (Serial1.available())
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

Este loop simplemente espera por un byte recibido por la UART y lo retransmite en la misma UART, la cual está configurada con un pin de dirección por hardware (RTS manejado por el transmisor).

Si pudiera usar el controlador DMA (Direct Memory Access) del Teensy para hacer la retransmisión, podría vaciar el código loop y  utilizar el Teensy para otras tareas sin afectar el rendimiento de la retransmisión.

Los requerimientos para el DMA son bien simples:

  • Gatillar un requerimiento DMA cuando 1 byte es recibido por la UART.
  • La transferencia DMA tiene que ser de 1 byte.
  • La transferencia DMA debe leer desde el registro de DATA de la UART (data recibida).
  • La transferencia DMA debe escribir en el registro de DATA de la UART (para retransmisión).
  • No se tiene que involucrar la CPU en el proceso, no debe necesitarse eventos de interrupción o pooling.

Mirando a las capacidades del DMA de la CPU ARM MK20DX256VLH7 del Teensy, se ve factible, quedando operativo después de algunas pruebas.

Ahora la retransmisión tiene una latencia menor y un timing más parejo:

Teensy with UART DMA
Teensy con UART manejada por DMA

Las señales en el gráfico son:

  • RX del Teensy (viene del TX del Odroid).
  • TX del Teensy.
  • Pin de Dirección por Hardward.

La codificación ahora tiene un setup más grande para configurar el DMA y la UART (la API serial del Teensy ya no es útil), pero la función loop() ahora está vacía:

#define UART_TXRTSE (2)
#define UART_TXRTSPOL (4)

#define BAUD_RATE (1000000)

void setup() 
{
  int divisor = BAUD2DIV(BAUD_RATE);  

  // DMA:
  
  // p 415 source address = uart data register
  DMA_TCD1_SADDR = &UART0_D;

  // p 415 source address offset
  DMA_TCD1_SOFF = 0;

  // p 416 transfer attributes: 8 bits
  DMA_TCD1_ATTR = 0;

  // p 417 minor byte count = 1 byte
  DMA_TCD1_NBYTES_MLNO = 1;

  // p 420 last source address adjustment = 0
  DMA_TCD1_SLAST = 0;

  // p 420 destination address = uart data register
  DMA_TCD1_DADDR = &UART0_D;

  // p 421 destination address offset
  DMA_TCD1_DOFF = 0;

  // p 423 channel link disabled
  DMA_TCD1_CITER_ELINKNO = 1;

  // p 423 last destination address adjustment = 0
  DMA_TCD1_DLASTSGA = 0;

  // p 427 channel link disabled
  DMA_TCD1_BITER_ELINKNO = 1;
  
  // p 424 control and status = 8 cycle stall, active
  DMA_TCD1_CSR = DMA_TCD_CSR_BWC(3) | DMA_TCD_CSR_ACTIVE;

  // p 402 enable DMA REQ channel 1.
  DMA_SERQ = DMA_SERQ_SERQ(1);

  // clock setup
  // p 252-259 system clock gating
  SIM_SCGC6 |= SIM_SCGC6_DMAMUX;
  SIM_SCGC7 |= SIM_SCGC7_DMA;
  SIM_SCGC4 |= SIM_SCGC4_UART0;
  
  // wait for clocks to become stable.
  delay(500);

  // p366 dma mux channel configuration  
  DMAMUX0_CHCFG1 = DMAMUX_ENABLE | DMAMUX_SOURCE_UART0_RX;

  // UART:
  
  // p 1222 UART0 Control Register 5 request DMA on receiver full
  UART0_C5 = UART_C5_RDMAS;

  // RX TX pins
  CORE_PIN0_CONFIG = PORT_PCR_PE | PORT_PCR_PS |
                     PORT_PCR_PFE | PORT_PCR_MUX(3);

  CORE_PIN1_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE |
                     PORT_PCR_MUX(3);

  // p 1208 uart0 baud rate  
  UART0_BDH = (divisor >> 13) & 0x1F;
  UART0_BDL = (divisor >> 5) & 0xFF;
  UART0_C4 = divisor & 0x1F;

  UART0_C1 = UART_C1_ILT;
  UART0_TWFIFO = 2; // tx watermark
  UART0_RWFIFO = 1; // rx watermark
  UART0_PFIFO = UART_PFIFO_TXFE | UART_PFIFO_RXFE;

  UART0_C2 = UART_C2_TE | UART_C2_RE | UART_C2_RIE;

  // enable PIN 6 as hardware transmitter RTS with active HIGH.
  CORE_PIN6_CONFIG = PORT_PCR_MUX(3);
  UART0_MODEM = UART_TXRTSE | UART_TXRTSPOL; 
}

void loop() 
{
}

En realidad, por ahora ejecuto el típico ‘blink’ en la funcion loop() para saber que el Teensy está corriendo.

Nuevo Driver Dynamixel con un Teensy 3.2

Reemplacé my prototipo TTL inicial con un Teensy 3.2, éste es una placa de desarrollo con una CPU ARM de 32 bits y 72MHz en un formato pequeño de 35×18 mm.

Odroid-XU4, Teensy 3.2 and Servo MX-28 Setup
Setup de Odroid-XU4, Teensy 3.2 y Servo MX-28

Seleccioné esta placa porque tiene varias interfaces seriales (UART) que soportan:

  • 1 y 3 Mbps, velocidades que pueden ser usados con el servo MX-28.
  • Un pin de dirección por hardware. Su RTS por hardware puede indicar cuándo la UART está transmitiendo (al configurar que el RTS sea generado por el transmisor de la UART en vez del receptor de la UART), lamentablemente el Odroid-XU4 (como varias otras plataformas) no soporta esta opción por lo que he visto de la documentación disponible.

También el sitio web del Teensy se ve bastante bueno.

Robotis documenta este setup para conectar una UART al bus Dynamixel. Requiere una UART, un pin de dirección y lógica de 5V:

Robotis Citcuit Interface to Dynamixel Bus
Interfaz de Robotis para Bus Dynamixel

Así, al poner el Teensy 3.2 entre el Odroid-XU4 y el bus Dynamixel, puedo generar el pin de dirección por hardware en vez de una implementación por software que puede tener problemas de timing.

Este es un esquema simplificado del setup, cambié el buffer de recepción por una puerta OR para evitar colocar una resistencia pull-up:

Odroid-XU4, Teensy 3.2, Dynamixel Schematic
Esquemático Odroid-XU4, Teensy 3.2, Dynamixel

Este esquemático usa sólo una UART del Teensy. La función del Teensy es sólo retransmitir al bus Dynamixel y generar el pin de dirección. Mientras tando la señal de retorno del Bus va directo al Odroid, no hay necesidad de pasarla por el Teensy. De esta forma, este setup se puede usar con otros micro-controladores que sólo cuentan con 1 UART, además que no hay retardo extra en el retorno. El Odroid provee 1.8V y 5V que alimentan al Teensy y a los level shifters y el Teensy provee 3.3V que también alimenta a los level shifters.

Probablemente mi setup final use 2 UARTs del Teensy, de forma que éste pueda generar un retorno al Odroid y participe como otro dispositivo en el bus Dynamixel (siguiendo su protocolo) de forma que cumpla alguna función como PWM o I/O análogo. Dependerá de si sobra suficiente tiempo libre en el bus para agregar más comandos, que ya está bien limitado con los 8ms del ciclo de control del software de DARwIn-OP.

La Documentación para la UART del sitio de Teensy es bien clara y se programa con un add-on IDE de Arduino llamado Teensyduino.

El siguiente programa (que presenta problemas como se menciona luego) se deriva fácilmente de la documentación para retransmitir a través de la interfaz serial y con un pin de dirección:

void setup() 
{
  Serial1.begin(1000000);
  Serial1.transmitterEnable(6);
}
void loop() 
{
  if (Serial1.available()) 
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

En este ejemplo, el pin 6 es configurado como pin de dirección para señalar cuándo hay una transmisión en proceso.

En el lado del Odroid-XU4, se puede usar la librería estándar de Dynamixel, sólo se necesita cambiar el nombre del dispositivo serial que corresponde a /dev/ttySAC0 para la UART del conector 10 expansión del Odroid.

Esta configuración con un servo MX-28 a 1Mbps funciona. Pero 2 cosas no funcionan como planificado:

  • Hay un retardo de como 5 bytes (50 us) en la retransmisión. Yo estaba esperando sobre 1 byte pero no tanto.
  • El pin de dirección 6 no funciona correctamente siempre (esto no se notó incialmente).

Retardo de Retransmisión

Teensy Retransmission Delay
Retardo de Retransmisión en Teensy

La Documentación de la CPU MK20DX256VLH7 del Teensy describe en el capítulo 47 la interfaz UART. En la sección 47.3.21 describe el registro UART_RWFIFO que configura el umbral del buffer de recepción para notificar a la CPU vía interrupción, su valor es 1 después del reset.

Al revisar el código fuente de la librería Serial1.begin(), se ve que este umbral UART_RWFIFO es incremendado a 4. Esto permite bajar el uso de CPU al recibir data, pero agrega latencia. También, la librería maneja la UART vía interrupciones así que la CPU sabe que llega data sólo después de la recepción de los primeros 4 bytes (si se hubieran transmitido menos de 4 bytes también se notifica a la CPU por una interrupción de inactividad). La función Serial1.available() no interroga la UART, sino que revisa unos buffers en memoria que son llenados vía interrupciones.

Como estoy usando el Teensy sólo para retransmitir, bajé el umbral de vuelta a 1 byte modificando la función setup así:

void setup() 
{
  Serial1.begin(1000000);
  Serial1.transmitterEnable(6);

  // set receiver buffer threshold for interrupt back to 1.
  uint8_t c2 = UART0_C2;
  UART0_C2 = c2 & ~UART_C2_RE; // disable C2[RE] (receiver enable)
  UART0_RWFIFO = 1;            // set receiver threshold
  UART0_C2 = c2;               // restore C2[RE]
}
void loop() 
{
  if (Serial1.available())
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

UART0_C2 y UART0_RWFIFO apuntan a los registros hardware de configuración y están definidos en las librerías de Teensy. La UART #0 de la CPU corresponde al objeto C++ Serial1.

Pin de Dirección con Falla

Después de hacer un test de estrés, 1 de cada 30 a 100 comandos al servo resultaban en un timeout esperando respuesta del servo. Después de varios intentos, logré capturar algunos casos de falla en la señal del pin de dirección:

Direction Pin Failure
Falla 1 Pin de Dirección – El servo responde, pero hay un byte de retorno inicial extra.
Direction Pin Failure 2
Falla 2 Pin de Dirección – El servo recibe el byte #4 corrupto y no responde.

En los diagramas de captura digital, las señales son:

  1. TX del Odroid-XU4
  2. TX de retransmisión del Teensy 3.2
  3. Pin de dirección del Teensy 3.2
  4. Bus Dynamixel
  5. RX de vuelta al Odroid-XU4

Normalmente, el pin de dirección funciona bien pero a veces se desactiva durante la transmisión. La falla 1 muestra un caso que no afecta al mensaje al servo, pero el Odroid recibe un byte 0xFF extra inicial. La falla 2 muestra un mensaje que se corrompe, el 4o byte en el bus dinamixel vale 63 pero debería valer 5.

Durante mi chequeo previo de la librería de Serial1, noté que la funcionalidad de transmitterEnable es implementada por software y no está usando la capacidad del RTS de hardware de la UART. Del análisis de señal se deduce que esta implementación por software no es correcta, como me interesa una solución por hardware, no intenté arreglar la librería, pero al menos noté un caso de carrera crítica no manejada apropiadamente.

Así que luego de revisar nuevamente la Documentación de la CPU ARM MK20DX256VLH7, encontré la solución por hardware en la sección 47.3.14, el registro de configuración UART_MODEM describe cómo configurar el RTS para indicar cuándo el transmisor de la UART está activo.

También, en este otro Documento ARM K20 se describe las múltiples configuraciones de pines de hardware. En el capítulo 8.1, lista cómo las diferentes señales internas de la CPU se pueden exponer en los pines externos. En particular, los pines de la CPU 25, 37 y 61 pueden ser configurados como RTS de la UART 0. Estos son pines de CPU y no de la placa Teensy. Este esquemático muestra que sólo 2 pines están disponibles en la placa: el pin 6 (corresponde al pin 61 de la CPU) y el pin 19 (pin 37 de la CPU). Después de buscar un poco más encontré ejemplos de código de cómo programar la configuración de un pin, en particular, el pin 6 para RTS (funcionalidad ALT3).

Esta es la versión final del setup que usa un pin de dirección por hardware, la llamada a la API Serial1.transmitterEnable() fue eliminada.

#define UART_TXRTSE (2)
#define UART_TXRTSPOL (4)

void setup() 
{
  Serial1.begin(1000000);
  
  // set receiver buffer threshold for interrupt back to 1.
  uint8_t c2 = UART0_C2;
  UART0_C2 = c2 & ~UART_C2_RE; // disable C2[RE] (receiver enable)
  UART0_RWFIFO = 1;            // set receiver threshold
  UART0_C2 = c2;               // restore C2[RE]

  // enable PIN 6 as hardware transmitter RTS with active HIGH.
  CORE_PIN6_CONFIG = PORT_PCR_MUX(3);
  UART0_MODEM = UART_TXRTSE | UART_TXRTSPOL; 
}
void loop() 
{
  if (Serial1.available())
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

El pin de dirección funciona correctamente y tiene mejor consistiencia en los timings:

Teensy Driver Working
Driver Teensy funcionando bien

Ahora me falta reducir este prototipo a una placa debajo del Teensy 3.2.

Nuevo driver para Dynamixel

Como estoy usando un Odroid-XU3 (y pronto voy a actualizarlo con un Odroid-XU4) con un adaptador USB2AX para conectar los servos Dynamixel MX-28, me ha estado molestando el retardo USB del adaptador.

El USB2AX es un dispositivo USB 1.1 y el USB 1.1 tiene un entramado de 1ms para transmitir y recibir data. Cualquier
comando dynamixel toma al menos 2ms lo cual no es aceptable. Incluso usando los comandos bulk de lectura y escritura (los que toman incluso más tiempo en la medida que conecto más servos) me gustaría cumplir con el ciclo de control de 8ms del programa original del DARwIn-OP y me estoy acercando al límite.

Por suerte, como se describe en foro odroid, el UART del Odroid se puede configurar a la especificación del bus Dynamixel (8 bit, 1 stop, No Parity) a 1Mbps e incluso hasta 3Mbps!!!

Por lo que estoy trabajando en un driver para implementar la interfaz serial TTL half-duplex de Dynamixel con sólo los pines RX and TX del Odroid (sin usar ningún pin de control para comandar el half-duplex). De esta forma, se pueden usar UARTs simples como la que tiene el Odroid, que no tiene una salida por hardware para indicar una transmisión en proceso (y no quiero usar un gpio controlado por software para evitar errores de timing).

Por ahora hice un prototipo TTL (la UART del Odroid es de 1.8V, mientras que el bus Dynamixel es de 5V), pero voy a continuar reduciendo esto con un micro-controlador.

TTL servo driver prototype
prototipo TTL de driver de servo

Antes de esto estaba trabajando en usar un UART SPI para reemplazar el USB2AX, pero eso se va a la basura ahora.

Pausa 2

Nuevamente después de otra pausa de verano estoy retomando este proyecto de mi clon de DARwIn-OP.

Actualmente estoy mejorando mi fresadora Sherline con comando digital de revoluciones y estoy reemplazando el tornillo del eje Y que ya está muy desgastado y empezó a notarse en los últimos cortes.