Finalmente el proyecto fue presentado en Julio.
Aunque finalmente no conseguí implementar el piloto automático todo lo demás funcionó como se esperaba.
Añado un video del sistema funcionando (se ve un pequeño problema con el aterrizaje pero ya está solventado)
Finalmente y tras no pocos dolores de cabeza resultado de la mezcla explosiva de latencias, endian orders y velocidades de transmisión me dispongo a hacer el esqueleto de lo que será quizá antes de la semana que viene el piloto automático.
Lo primero que tengo que hacer es convertir las coordenadas a decimales y posteriormente calcular la distancia entre dos coordenadas y el bearing.
Obtenido el bearing lo más fácil es establecer primero el heading y posteriormente tirar para adelante. En cada punto nuevo calcularemos la distancia y rumbo a seguir hasta el destino. Aún no tengo muy claro como sabré que ha llegado a su destino. Supongo que lo que haré será mirar si está dentro de un área en los alrededores del waypoint y si lo está pasar al siguiente waypoint.
Aquí hay una página con bastante información:
He implementado el algoritmo que mencionaba en el post anterior. Aún así he tenido que cambiar la frecuencia de actualización de los datos de los sensores porque se formaba un cuello de botella. En la primera imagen tenemos la latencia sin GPS y en la segunda tenemos la latencia con el GPS.
Nótese que la latencia aumenta durante los periodos de envío de los datos del GPS. Esto ocurre porque el transmisor tiene que esperar.
Aumentando el tiempo de muestreo le damos más tiempo para descansar
Aquí tenemos el muestreo a 8ms y una segmentación de datos de 8bytes. Es decir se van enviando los datos del GPS en bloques de 8, más los datos del protocolo.
A continuación 9ms de muestreo y segmentación de datos a 8bytes
Me quedo con esta, por la relación calidad/muestras. Ahora tendré que tunear el PID para la nueva frecuencia de muestreo. Y probar si todo funciona correctamente.
Tras implementar el GPS descubro que empiezo a tener problemas de latencia variable. La latencia se mantiene en 8ms pero aparecen picos de unos 20ms.
En el programa hay un temporizador que envía los datos de los sensores cada 4ms y si hay datos GPS envía los del GPS justo después. Los datos de los sensores tardan 2.7ms en enviarse pero los del GPS tardan casi 12 ms!!!!
Pongo un esquema hecho en paint para entender mejor el asunto
Si transmito todo justo después (es lo más normal) el puerto serie se queda ocupado durante 12 ms más. El temporizador después de los 4ms vuelve a saltar y se encuentra el puerto serie ocupado y ¡se pone a esperar perdiendo 3 transmisiones de datos! La solución es fácil de decir pero difícil de implementar.
Ampliar la velocidad del puerto no puedo porque el conversor serie da problemas con el full duplex y acaba siendo peor el remedio que la enfermedad. Esperar no es aceptable pues introduce latencia variable y poner otro puerto serie aumenta la complejidad del sistema.
Sólo queda por tanto una opción: dividir la información del GPS en bloques más pequeños y transmitirlos en el slot de tiempo que nos queda libre. La velocidad de actualización de datos del GPS es menor que la de los sensores y actualmente no hay ningún módulo comercial que pase de los 10Hz.
Calculo el número de slots que necesito y me da 10 slots. Utilizando 10 slots tendría un frecuencia de actualización de 25Hz. Utilizaré 15 slots que me dará un frecuencia aproximanda de 16Hz así me aseguro que no hay problemas con la señalización y que el puerto serie está disponible para el próximo ciclo.
Ahora que el quadrotor vuela mas o menos estable toca ponerle GPS para que pueda volar autónomo siguiendo una ruta.
La idea de implementación es la siguiente:
Visto esto paso a implementarlo…
Me ha costado pero al final ya tengo la brújula montada y funcionando.
En resumen si usamos el conector de la Navi-Ctrl (el de abajo no el frontal de la MK3MAG) lo conectaremos de la siguiente manera a la FC y obtendremos conexión I2C con la brújula.
Para leer de la brújula:
Para escribir en la brújula
Para la generación del CRC usaremos la fórmula siguiente
Ahora estoy probando fusionar los datos de la brújula con el algoritmo DCM
Parece que ya he conseguido recibir datos de la brújula con la placa adaptadora que he fabricado esta tarde.
La brújula funciona con I2C y he tenido que implementar la lectura a través de I2C porque en la Fligth-Ctrl está implementado por PWM.
El protocolo es sencillo pero un poco rebuscado.
La dirección de escritura de la brújula es 0×50. Para lectura siempre es dirección de escritura +1, es decir 0×51.
Para leer un los datos hemos de usar el comando 0×02. El proceso sería el siguiente:
Ahora sólo falta implementar la calibración por I2C y enviar los datos junto con los demás sensores y…
a por el GPS!
Ayer probé el último método que me quedaba por probar de entre los 3 que mencioné para pasar los datos entre la Fligth Ctrl y la Navi-Ctrl y no dió muy bueno resultados. Pérdida de muestras y latencia variable principalmente.
Hoy he decidido hacer un interfaz por arduino para que leyera los datos de la brújula y del GPS. Para la brújula realmente no haría falta pero para el GPS necesitaba otra UART. Cual ha sido mi sorpresa cuando estaba montando la placa adaptadora de arduino que la Fligth-Ctrl dispone de 2 UART!!
He dejado lo que estaba haciendo y me he puesto a fabricar una plaquita adaptadora para leer los datos de la brújula por I2C y los del GPS por la siguiente UART. Las conexiones están bien hechas y se encienden todas las placas pero aún falta la parte de programación.
Espero tenerlo listo en máximo un par de días pues ya he perdido mucho tiempo con la brújula y el GPS y aún tengo que trabajar la parte de control.
Voy a mencionar las posibilidades que tengo para pasar los datos de la Navi-Ctrl (brújula y GPS):
La primera y la mejor:
pasar los datos por el puerto SPI y mezclarlos en la Fligth-Ctrl. Los datos de brújula y GPS son de poca frecuencia por lo que pasar los datos por la Navi-Ctrl supone un aumento de la latencia, de esta manera la Fligth-Ctrl iría conectada directamente. Problema? Que el puerto SPI no funciona
La segunda:
leer los datos en un buffer y añadir datos de brújula y GPS. Esta es una buena manera de hacerlo, pero introduce muchísima latencia. Hay que parar los datos cada vez y añadir los nuevos y posteriormente volver a enviarlo. El incremento en la latencia es considerable (15 a 20 en lugar de 8 y en ocasiones puede ir a mas de 200ms). Pero el problema principal no es la latencia en sí sino la latencia variable, que desastibiliza el quadrotor
La tercera y que voy a probar:
leer los datos de brújula y GPS, enviarlos a la Fligth-Ctrl y una vez ahí empaquetarlos con los datos de los sensores y enviarlos a la Navi-Ctrl de nuevo. La gracia de este sistema es que no hace falta que la Navi-Ctrl retenga los datos, desde la misma interrupción puede enviar lo datos a la UART siguiente.
Espero obtener resultados que ya llevo 5 días perdidos con el asunto…
Estas placas se rompen solo de mirarlas. Por lo visto falla el puerto SPI. Cambiar el micro no es viable pues no tengo el programador.Bajo circunstancias normales la placa no serviría pero aún le puedo dar algún uso, redirigir la comunicación por la UART en lugar del puerto SPI.
Llevo varias horas peleándome con la UART pues tengo paquetes malformados que pasan el control de CRC. Sospecho que al reconstruir el paquete en la Navi-Ctrl lo hace mal, pero todavía quedan un par de horas de Debug antes de confirmar nada…
