Controladora para QUAD con 18F4550

[elgarbe]

Bueno, mientras descanso un poco con el OSD y espero a comprar una nueva LPCXpresso (queme la que estaa usando  ) decidí empezar con el control de un quadricoptero basada en el 18F4550. Podría haver seguido la línea de los ARM, pero como tengo varios proyectos pendientes con PIC y necesito "entrenamiento" decidí hacerlo con este micro para ir aprendiendo a usarlo, usando XC8.
Tambien tengo un presedente, un amigo, Martín, consiguio hacer una controladora con dicho micro, por lo tanto ya hay algunas ideas para arrancar.

Vamos a dividir el desarrollo en varias partes que luego uniremos:

• Quad
• Hardware - Controladora
• 4x Software PWM
• Lectura Rx de RC
• Lectura Acelerómetro (ADXL345)
• Lectura Giróscopo (L3G4200)
• Fusion de datos para obtener horientación
• Control PID

Muchas de estas cosas ya las tenia funcionando en el LPC1769, así que las ideas están, solo hay que portarlas a la arquitectura PIC.

Iré poniendo en cada post cada uno de los temas a desarrollar.

Saludos!

[jhozate]

Atento a este hilo !
Cambio y fuera! 

[elgarbe]

QUAD

El frame del quad fue una especie de regalo de un amigo del foro-aeromodelismo.com.ar (me lo vendio a $5). Es un DJI original, tiene 400mm de distancia entre brazos.

Los motores son comprado en RCTimer. Los compre en noviembre del año pasado y me llegaron hace 1 mes, jejeje. Son HP2812 de 880 KV. Los variadores (ESC) son tambien de rctimer con el firmware de simonk.

Veamos unas fotitos:







Bueno, veamos un poco como es el tema del montaje de los elementos.

Primero soldamos las alimentaciones de los ESC a la placa distribuidora de tension que trae impreso el frame del DJI:

Estañamos bien la placa de distribucion


Estañamos bien los cables


Soldamos


Trabajo terminado


Luego vamos a instalar los motores y los ESC. Los primeros van con tornillos y los segundos los he puesto con presintos:





Luego debemos soldar las salidas de los ESC a los motores, previo a chequear el sentido de giro de cada motor. Debemos dejar 2 en sentido horario y 2 en sentido antihorario, de esta forma, se anula la fuerza "giratoria" que se genera por el giro de los motores. Si esta precausion el QUAD giraría constantemente y no podríamos controlarlo. Acá podemos tambien poner conectores si quisiéramos, en mi caso preferí soldar directamente los cables:



En este momento me estan faltando las hélices, ya que 2 son CW y 2 son CCW. Esta semana si me llegan edito este post y lo termino con el montaje de las mismas.

Saludos!

[elgarbe]

Hardware - Controladora

Bueno, este fin de semana largo estuve trabajando en la controladora. Siempre que encaro un desarrollo, aunque sea por hobby, trato de que me quede algo, aprender o probar algo nuevo. Así que en etste caso decidi trabajar con un 4550 con encapsulado TQFP, de este modo voy a provar de rutear, fabricar y soldar una placa con pistas bien finas (15 mil) que, de paso, sería el comienzo de las pruebas para poder usar el LPC1769 en algún proyecto.

Bueno, la idea de la controladora es, micro 18F4550 TQFP, cristal externo de 8MHz y trabajar con el PLL para obtener 48MHz, entrada de alimentacion en 7-12V (2S o 3S de baterias de LiPo), 4 PWM por software para manejar los esc (ampliable a , Entrada para leer la señal PPM del Rx del radio control, conexion a módulo bluetooth de microingenia por uart y conexion de la IMU GY-80, la cual tiene giroscopo, acelerometro y magnetometro de 3 ejes y barometro para usarlo como altímetro.

Bueno, despues de varias horas de diseño, este es el esquemático:



y el PCB



Para la fabricacion del PCB uso el método fotográfico, sensivilizo un PCB con pintura serigráfica (de GCTech), luego imprimo los fotolitos con una imresora chorro de tinta común, en papel transparencia y en la mejor calidad. Si bien no queda perfecto, sirve para hacer prototipos. Con esta placa de pistas finas he notado que las pequeñas pintitas sin tinta que queda en la transparencia jode un poco en el producto final. Luego expongo con una insoldaora UV (con LED's), revelo y al percloruro. Luego la máscara antisolder tambien serigráfica y foto sensible:



Luego el perforado y finalmente la aplicacion de pasta de estaño. Esta la aplico con una jeringa casera, llena de estaño en pasta (smtsolutions) y con un acople para inyectar aire comprimido. En la punta tiene una aguja tambien comprada en SMT solutions. Tengo una pequeña electroválvula que me permite dosificar el estaño:







El montaje de los componentes los hago con una pequeña pinza tipo las de depilar:



Luego se va al horno de reflow:



En este caso use el horno de produccion 



Placa soldada:



Hay algunos cortos en el uC por que aplique mucha pasta. Mas adelante voy a mostrar como hacer un stencil para aplicar bien la pasta. Para este tamaño no se justifica.



Luego monto los componentes TH:



placa terminada:



Bueno, ya tengo la placa para hacer las pruebas.

Tengo que aclarar que al esquemático que estoy usando le faltan muchos detalles, como ser medicion de tension de batería, protecciones y otras cosas, pero necesitaba un comienzo para las pruebas. Tambien tengo que comentar que todo el software lo fui probando con la entrenadora 3.0 de Felixls, a la cual le hice una MCU card para el 4550 y que tengo que compartir en estos días. Gracias a las pruebas con la entrenadora, pude ver algunas cuestiones de hardware.

Bueno, si alguien quiere el proyecto en altium me avisa y lo comparto, aunque lo ideal sería esperar a una version más terminada...

Saludos y seguimos con el soft!

[AngelGris]

  Sigo atento el hilo... el aeromodelismo es una asignatura pendiente en mí. Llegué a hacer una entrenador de ala alta en derpon (en realidad era tergolina verde comprada en una librería) el cuál volé un par de ocasiones en Rauch. Luego corté piezas sí en depron para un modelo que apareció en "El aeromodelista" pero nunca lo terminé.

  Los mio es luchar mucho y cuando veo que algo tiende a funcionar ya lo dejo  Me pasa con todos los proyectos.

[pajaro]

Hola compañeros
parece que el amigo elgarbe no para,...jejeje
me apunto al hilo...

[elgarbe]

Gracias gente por los comentarios, es bueno ver que alguien lee lo que uno pone, jeje, da ganas de seguir escriviendo.

Saludos!

[elgarbe]

PWM

Para el quad son necesarios 4 PWM para gobernar los ESC. Vamos a crear el código para 8 PWM para tener la posibilidad de PAN and TILT de una camarita y para llegar hasta a un hexa. Aparte porque no agrega dificultad tener 8 PWM si ya tengo 4.
Vamos a usar el puerto D del 18F4550 para este fin.
La idea del PWM es:

• Configurar el micro para que corra a 48MHz. Con esto tengo un Tick de cualquier timer de 0.08333 useg.
• Vamos a suponer de entrada que la señal stanadar del PWM es con una frecuencia de 50Hz y con un ancho mínimo de 1mseg y un ancho máximo de 2mseg. Los otros 18 mseg son tiempo muerto. En realidad, en aeromodelismo, en los Rx de RC no existe ese tiempo muerto, ya que los PWM de cada canal están desfasados unos de otro y con 10 canales se cubren los 20mseg. Pero en nuestro caso podemos poner todos los canales en 1 al empezar y luego ir bajando uno a uno de acuerdo al duty cicle de cada canal.
• Por lo tanto el SPAN de tiempo es de 1000 useg.
• Supongamos queremos tener 200 pasos. Cada paso sería de 5 useg.
• En principio, tendríamos el timer interrumpiendo cada 5 useg y dentro de la interrupción verificamos si un contador de interrupciones es igual a cada duty cycle. Si se igualan, pongo en 0 esa salida. Cuando entro a la interrupción con el contador en 0 pongo todas las salidas en 1. De este modo el micro estaría interrumpido constantemente, cada 5 useg… es poco probable que algo así funciones…

• Una optimización posible es, sabiendo que una vez que llegué a los 2mseg, el PWM va a estar en 0 durante 18 mseg, ya no interrumpir cada 5 useg, sino que configuro el timer para que interrumpa a los 18 mesg.
• Teniendo en cuenta esto veo que necesito un timer que, con un tick de 0.0833 useg, pueda interrumpir cada 5 useg y también a los 18 mseg, dependiendo de con que valor precargo al contador del  timer.
• Sin hacer muchas cuentas sabemos que el timer debe ser de 16 bits. Por lo que puede contar 65535 ticks. Si cada tick es de 0.08333 useg podría contar hasta 5.4 mseg. Entonces vemos que debemos poner un prescaler en el timer para dividir los ticks por 4.
• Con un tick de 0.08333 * 4 = 0.333 useg, el timer puede llegar a contar hasta 21.8 mseg. Lo cual esta de diez. Luego verificamos que con un tick de 0.333 useg necesitamos contar hasta 15 para obtener los 5 useg de resolución.
• 15 cuentas me parece muy poco, por lo que voy a tomar otra opción.
• Tomo el prescaler de 2. Por lo tanto el tick del timer será 0.1666 useg. Tendré 30 cuentas para llegar a 5 useg y podré llegar a 11 mseg. Entonces, cuando pasen los 2 mseg útiles del pwm, lo que hago es configurar el timer para que interrumpa a los 9 mseg. Cuando lleguen 2 de esas interrupciones es que ya pasaron los 20 mseg y puedo arrancar otra vez.
• Otro punto de optimización es, en vez de poner 8 “if” para ver si ya tengo que poner a cero el PWM en cuestión (if(cont_int == duty[0]), agarro un array en memoria de 200 bytes. Cada bit de cada elemento del array representa el estado de las 8 salidas en cada instante. Los 200 elementos son los 200 pasos de resolución que queremos obtener. Entonces en cada interrupción simplemente movemos el elemento del array al puerto directamente!!!
• Una mejora a esto es usar la función XOR. Entonces si quiero que en el paso 100 la señal de un PWM se vaya a cero tendría que poner 100 1’s en el primer bit de los primeros 100 elementos del array y luego 100 0’s. bueno, si en vez de mover el valor del array uso la función XOR, puedo poner un 1 solo en el lugar donde quiero que se produzca el cambio. Todo esto quedará mas claro en el código implementado.
• Bueno, después las pruebas concluyo que a 48MHz no puedo tener resolución de 5 useg ya que las instrucciones dentro de la interrupción demoran ese tiempo. Entonces paso a resolución de 10 useg y por lo tanto 100 pasos.
• Otra opción sería durante el mseg que dura el cambio de pwm, tomar la CPU y no dejarla hacer otra cosa que no sea el PWM, entonces puedo darle el tiempo que quiero ya que no espero a que vuelva a entrar la interrupción, sino que me quedo dentro de la interrupción por ese milisegundo. Es una opción interesante para analizar. Incluso tengo código en asm para estudiar (http://www.electro-tech-online.com/threads/drive-33-servos-with-one-pic-usart.34390/). El tema es que durante ese milisegundo (o 2) la CPU no puede hacer nada más que el PWM.
• El código que tengo funcionando hasta ahora es este;
  
  

Código: [Seleccionar]

#define TMR_10us    65535 - 32      // Valor a cargar en el TMR para 10 useg
#define TMR_1ms     65535 - 6000    // Valor a cargar en el TMR para 1 mseg
#define TMR_9ms     65535 - 53930   // Valor a cargar en el TMR para 9 mseg

// Variables globales.
uint8_t cont_int = 0;
uint8_t dutys_buff[100];    //Buffer donde se almacenan cuando deben ir a off las salidas
uint8_t dutys[8];           //Duty cicle de cada salida PWM

// FUNCION DE INTERRUPCION
void interrupt ISR(void) {
    if(INTCONbits.TMR0IF){
        uint8_t i, mask;
        INTCONbits.TMR0IF = 0;      // Limpiamos bandera de desborde
        switch(cont_int){           // Verifico en que interrupcion estoy
            case 0:                 // Es la primera
                TMR0 = TMR_1ms;     // Ajusto para que proxima int sea en 1 mseg
                PORTD = 0x0F;       // Pongo en 1 las 4 salidas.
                mask = 1;           // Preparo el buffer a usar para hacer el toggle
                for(i=0;i<8;i++){   // de la salida correspondiente segun el duty elegido
                    dutys_buff[dutys[i]] = dutys_buff[dutys[i]] | mask;
                    mask = mask << 1;
                }
                cont_int++;         // Incremento interrupcion
                break;
            case 101:               // Si estoy en la 101
                TMR0 = TMR_9ms;     // Configuro para interrumpir en 9 mseg
                PORTD = 0;          // Serían 11 mseg desde el comienzo
                cont_int++;         // Pongo todos los PWM a 0
                break;              // Incremento interrupcion
            case 102:               // Si es la 102
                TMR0 = TMR_9ms;     // Tengo que esperar otros 9 mseg para completar
                cont_int = 0;       // los 20 mseg. Reseteo el contador de interrupciones
                break;
            default:                // Si es la int 1 a 100
                TMR0 = TMR_10us;    // Ajusto para que proxima int sea dentro de 10 useg
                PORTD ^= dutys_buff[cont_int - 1];  // Si corresponde pongo a 0 la salida
                cont_int++;         // Incremento el contador
                break;
        }
    }
}


• Para este proyecto, el QUAD, seguramente con 100 pasos servirá. En caso de necesitar mayor resolución y más canales, se puede pensar en un micro que haga solo PWM y reciba comando por I2C o UART. Pero esto creo que sería, por ejemplo, para el hexápodo, el cual lleva 3 servos por pata con un total de 18 servos. En el caso del quad, podemos resolverlo así sin problemas.
• Otro punto que alguno se preguntará es porque no usar un micro con 4 PWM. Bueno, el tema es que esos micros, la frecuencia mínima del PWM por hardware es de 240 Hz. En teoría para los ESC podría andar, pero no creo que un servo funciones con esa frecuencia. Sería cuestión de probar…

En el main.c tengo lo siguiente:

Código: [Seleccionar]

/*
 * File:   main.c
 * Author: elgarbe
 *
 * Created on 25 de abril de 2014, 12:40
 */
#define _XTAL_FREQ 48000000
#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <plib/usart.h>
#include "configuracion_de_fuses.h"
#include "configuracion_hard.h"

#define TMR_10us    65535 - 32      // Valor a cargar en el TMR para 10 useg
#define TMR_1ms     65535 - 6000    // Valor a cargar en el TMR para 1 mseg
#define TMR_9ms     65535 - 53930   // Valor a cargar en el TMR para 9 mseg

// Variables globales.
uint8_t cont_int = 0;
uint8_t dutys_buff[200];    //Buffer donde se almacenan cuando deben ir a off las salidas
uint8_t dutys[8];           //Duty cicle de cada salida PWM

// FUNCION DE INTERRUPCION
void interrupt ISR(void) {
    if(INTCONbits.TMR0IF){
        uint8_t i, mask;
        INTCONbits.TMR0IF = 0;      // Limpiamos bandera de desborde
        switch(cont_int){           // Verifico en que interrupcion estoy
            case 0:                 // Es la primera
                TMR0 = TMR_1ms;     // Ajusto para que proxima int sea en 1 mseg
                PORTD = 0x0F;       // Pongo en 1 las 4 salidas.
                mask = 1;           // Preparo el buffer a usar para hacer el toggle
                for(i=0;i<8;i++){   // de la salida correspondiente segun el duty elegido
                    dutys_buff[dutys[i]] = dutys_buff[dutys[i]] | mask;
                    mask = mask << 1;
                }
                cont_int++;         // Incremento interrupcion
                break;
            case 101:               // Si estoy en la 101
                TMR0 = TMR_9ms;     // Configuro para interrumpir en 9 mseg
                PORTD = 0;          // Serían 11 mseg desde el comienzo
                cont_int++;         // Pongo todos los PWM a 0
                break;              // Incremento interrupcion
            case 102:               // Si es la 102
                TMR0 = TMR_9ms;     // Tengo que esperar otros 9 mseg para completar
                cont_int = 0;       // los 20 mseg. Reseteo el contador de interrupciones
                break;
            default:                // Si es la int 1 a 100
                TMR0 = TMR_10us;    // Ajusto para que proxima int sea dentro de 10 useg
                PORTD ^= dutys_buff[cont_int - 1];  // Si corresponde pongo a 0 la salida
                cont_int++;         // Incremento el contador
                break;
        }
    }
}

void MyMsDelay(int ms);

void main(void) {
    uint8_t i;

//    unsigned char MsgFromPIC[100];

    ADCON1=0x0F; //Todos entrada/salida digitales. - equivalente en binario ADCON1= 0b00001111
    TRISA=0x00; //Todos como salida.
    TRISB=0x00; //Todos como salida.
    TRISC=0x00; //Todos como salida.
    TRISD=0x80;         //RC7 como entrada (RX de la usart)
    TRISE=0x00; //Todos como salida.              - equivalente en binario TRISE = 0b00000000

    T0CON = 0;          //16 bits, %2 prescaler, Timer apagado
//    TMR2 = TMR_ADJ;            // Ajuste para obtener 100useg por cada Interrupcion

    // (frecuencia a la que trabaja el cpu/dbaudrate)/16)-1
    // (48.000.000/115200)/16)-1 = 25
    OpenUSART(USART_TX_INT_OFF &
        USART_RX_INT_OFF &      //Sin interrupciones
        USART_ASYNCH_MODE &     //Modo asincrono (fullduplex)
        USART_EIGHT_BIT &       //8 bits de datos
        USART_CONT_RX &         //Recepción continua
        USART_BRGH_HIGH, 25);   //115.2 K Baudios

    INTCONbits.TMR0IF = 0;
    INTCONbits.TMR0IE = 1;      // Habilito int del Timer 0
    INTCONbits.GIE = 1;         // Habilito la int global

    for(i=0;i<100;i++)
        dutys_buff[i] = 0;
    
    //Hacer funcion de asignacion de duty!!!!!!!!
    dutys[0] = 0;               //Pongo el duty de la salida 1 en 1 mseg
    dutys[1] = 50;              //Pongo el duty de la salida 2 en 1.5 mseg
    dutys[2] = 100;             //Pongo el duty de la salida 3 en 2 mseg
    dutys[3] = 75;              //Pongo el duty de la salida 4 en 1.75 mseg
    
    MyMsDelay(500);
    T0CONbits.TMR0ON = 1;

    while(1){
        LED4=1;
        MyMsDelay(500);
        LED4=0;
        MyMsDelay(500);
    }
}

void MyMsDelay(int ms){
    while(ms--){
        __delay_ms(1);
    }
}

Bueno, primer tema resuelto, 4x PWM funcionando.

Saludos!

[elgarbe]

Leyendo Rx del RC

Bueno, la teoría de como leo la señal del receptor del radiocontrol la pueden encontrar acá:

http://www.todopic.com.ar/foros/index.php?topic=42110.msg349755#msg349755

Simplemente me he dedicado a portar las funciones al 18F4550 ya que las tenía en ARM.

Código: [Seleccionar]

/*
 * File:   main.c
 * Author: elgarbe
 *
 * Created on 25 de abril de 2014, 12:40
 */
#define _XTAL_FREQ 16000000
#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <plib/usart.h>
#include "configuracion_de_fuses.h"
#include "configuracion_hard.h"
//#include <types.h>
#define RX_CHANNELS 8


/* defines */
#define RX_SUCCESS     1
#define RX_NO_PACKET   0

/****************************************************************************/
/*                             Private Variables                            */
/****************************************************************************/
static unsigned int pulses[RX_CHANNELS];         //Matriz para guardarel tiempo de cada pulso de cada canal
static char packet_available; //Bandera para indicar que ya recibimos RX_CHANNELS pulsos

void MyMsDelay(int ms);
unsigned char rx_get_packet(unsigned int packet[RX_CHANNELS]);
// Variables globales.

// FUNCION DE INTERRUPCION
void interrupt ISR(void) {
    // persistent local variables
    static unsigned int tmp_pulses[RX_CHANNELS];
    static unsigned int prev_ticks = 0;
    static unsigned int current_pulse = 0;
   
    unsigned int curr_ticks, nanos, c;   // local variables

    if(PIR2bits.CCP2IF){
        curr_ticks = CCPR2H;      // compute delta time (each tick is 10ns)
        curr_ticks <<= 8;
        curr_ticks += CCPR2L;
        nanos = curr_ticks - prev_ticks;

        if (nanos > 12000){     //Si el tiempo del pulso supera los 3mseg
            current_pulse = 0;  //es que estamos en el pulso de sincronismo
        }
        else {
            tmp_pulses[current_pulse] = nanos;  // store this sample
            current_pulse++;                    // advance to next pulse
            if (current_pulse == RX_CHANNELS) {     // check for end of packet
                current_pulse = 0;              // wrap pointer
                for (c=0; c<RX_CHANNELS; c++)   // copy out packet
                    pulses[c] = tmp_pulses[c]/4;
                packet_available = 1;           // signal available packet
            }
        }
        prev_ticks = curr_ticks;    // save current as previous for next event
        PIR2bits.CCP2IF = 0;        // Limpiamos bandera del CCP
    }
}

void main(void) {
    unsigned char canal, MsgFromPIC[100];
    unsigned int rx_values[RX_CHANNELS];

    ADCON1=0x0F; //Todos entrada/salida digitales. - equivalente en binario ADCON1= 0b00001111
    TRISA=0x00; //Todos como salida-   - equivalente en binario TRISA = 0b00000000
    TRISB=0X00; //Todos como salida.              - equivalente en binario TRISB = 0b00000000
    TRISC=0X00; //Todos como salida.              - equivalente en binario TRISC = 0b00000000
    TRISD=0x00;         //D7 a D1 como salida D0 como entrada
    TRISE=0X00; //Todos como salida.              - equivalente en binario TRISE = 0b00000000

    LATD=0x00; //Leds apagados.-

    TRISCbits.RC6 = 0;      //TX pin set as output
    TRISCbits.RC7 = 1;      //RX pin set as input

    TRISCbits.RC1 = 1;      //CCP2 as Input

    T3CONbits.TMR3ON = 0;   //Stop de Timer
    T3CONbits.RD16 = 1;     //16 bits timer
    T3CONbits.T3CCP2 = 1;   //TMR3 is source of CCP2
    T3CONbits.T3CKPS = 0;   //Sin prescaler

    CCP2CONbits.CCP2M = 5;  //Capture mode: every rising edge

    PIR2bits.CCP2IF = 0;    //Cler ny pending Int

    PIE2bits.CCP2IE = 1;        // Habilito la int por CCP2
    INTCONbits.PEIE = 1;        // Habilito int de los perifericos. CCP2 es uno de ellos
    INTCONbits.GIE = 1;         // Habilito la int global


    LED1=1;
    MyMsDelay(2000);
    LED1=0;

    OpenUSART(USART_TX_INT_OFF &
        USART_RX_INT_OFF &      //Activar la interrupcion por la recepcion de dato del buffer de Rx del USART
        USART_ASYNCH_MODE &     //Modo asincrono (fullduplex)
        USART_EIGHT_BIT &       //8 bits de datos
        USART_CONT_RX &         //Recepción continua
        USART_BRGH_HIGH, 51);   //9600 Baudios
   
    T3CONbits.TMR3ON = 1;   //Strt de Timer

    MyMsDelay(500);


//
    while(1){
      //Verifico si hay datos nuevo provenientes del Tx
        canal=rx_get_packet(rx_values);
        if(canal == RX_SUCCESS){
            for (canal=0; canal<RX_CHANNELS; canal++){
                sprintf(MsgFromPIC, "%d, ", rx_values[canal]);
                putsUSART(MsgFromPIC);
            }
            sprintf(MsgFromPIC, "\r\n");
            putsUSART(MsgFromPIC);
        }
        LED1=1;
        MyMsDelay(10);
        LED1=0;
        MyMsDelay(10);
    }
}

void MyMsDelay(int ms){
    while(ms--){
        __delay_ms(1);
    }
}

//  If a packet has arrived since the last call, 'packet' is filled with
//  the contents of the packet and RX_SUCCESS is returned.  If no packet
//  is available, RX_NO_PACKET is returned.
//  The size of 'packet' must be at least RX_CHANNELS.
//  This is NOT thread safe.
unsigned char rx_get_packet(unsigned int packet[RX_CHANNELS]) {
    char avail, ch;

    // turn off timer2 capture interrupts while accessing packet_available
    PIE2bits.CCP2IE = 0;        // Deshabilito la int por CCP2
    avail = packet_available; // grab current value
    packet_available = 0; // set to zero
    PIE2bits.CCP2IE = 1; // Vuelvo a encender la Interrupcion

    if (avail) {                    // check if a packet is available
        for (ch=0; ch<RX_CHANNELS; ch++)            // copy packet
            packet[ch] = pulses[ch];

        return RX_SUCCESS;  // set as success
    }else{
        return RX_NO_PACKET;    // set as failure
    }
}

La idea es configurar el timer3 como contador y con el CCP2 como input capture leer la señal PPM proveniente del Receptor. Como esa señal se va armando y tarda 20mseg en completarse tengo una función para la lectura asincrona de los datos cuando los necesite.
Tener presente que los datos del RC serán 4: Pitch, Roll, Yaw y altitud. Ellos serán los Set Point de los PID que utilizaremos para controlar cada ángulo.

Saludos y hasta la próxima!

[elgarbe]

Lectura Acelerómetro (ADXL345)

Bueno, la teoria de este sensor ya la estuvimos viendo en este post:

http://www.todopic.com.ar/foros/index.php?topic=41980.0

En este caso, lo que tuve que hacer es modificar la liibrería para adaptar las funciones del bus I2C que difieren bastante del ARM a los PIC. En ARM toda la comunicacion esta hecha basada en interrupciones. En el PIC son funciones que hacen poolling sobre los distintos registros.
En este caso he usado las librerías que trae el XC8 en su carpeta plib.

El drive del acelerómetro me ha quedado así:

Código: [Seleccionar]

/*
 * ADXL345.h
 *
 *  Created on: 04/12/2013
 *      Author: elgarbe
 */

#ifndef ADXL345_H_
#define ADXL345_H_

#define ADXL345_R       0xA7        //Read Address
#define ADXL345_W       0xA6        //Write
#define ADXL345_D       0x32        //Data

#define ACC_SAMP_BIASS 600          //Número de muestras para calular biass

// Definición de funciones.

void ADXL345_init();
char ADXL345ReadByte(char address);
void BMP085WriteByte(char address, char data);
void ADXL345_read_data(short* _accel_X, short* _accel_Y, short* _accel_Z);
void ADXL345_GetBiass(float* _biass_X, float* _biass_Y, float* _biass_Z);

#endif /* ADXL345_H_ */


Código: [Seleccionar]

/*
 * ADXL345.c
 *
 *  Created on: 02/04/2014
 *      Author: elgarbe
 */
#include "ADXL345.h"
#include <plib/i2c.h>
#include <stdio.h>

extern void MyMsDelay(int ms);

/************************************************************************
* Read initial calibration constants
************************************************************************/

char ADXL345ReadByte(char address){

    // Variables locales.
    signed char data=0x00;

    StartI2C();
    WriteI2C(ADXL345_W);
    WriteI2C(address);
    StartI2C();
    WriteI2C(ADXL345_R);
    data=ReadI2C();
    StopI2C();

    return(data);
}

void ADXL345WriteByte(char address, char data){

    StartI2C();
    WriteI2C(ADXL345_W);
    WriteI2C(address);
    WriteI2C(data);
    StopI2C();
}

void ADXL345_init(){

    ADXL345WriteByte(0x2D, 0b00000000);
    MyMsDelay(10);
    ADXL345WriteByte(0x31, 0b00001011); // 1XXX Full res, XX11 +-2g
    MyMsDelay(10);
    ADXL345WriteByte(0x2C, 0b00001010); // 100Hz Data Output Rate
    MyMsDelay(10);
    ADXL345WriteByte(0x2D, 0b00001000);
    MyMsDelay(10);
}


void ADXL345_read_data(short* _accel_X, short* _accel_Y, short* _accel_Z){
    // Variables locales.
    char  msb=0x00;
    char  lsb=0x00;

    StartI2C();
    IdleI2C();
    WriteI2C(ADXL345_W);
    IdleI2C();
    WriteI2C(ADXL345_D);
    IdleI2C();
    RestartI2C();
    IdleI2C();
    WriteI2C(ADXL345_R);
    IdleI2C();
    lsb = ReadI2C();
    IdleI2C();
    AckI2C();
    IdleI2C();
    msb = ReadI2C();
    IdleI2C();
    *_accel_X = (256*msb) + lsb;
    AckI2C();
    lsb = ReadI2C();
    IdleI2C();
    AckI2C();
    IdleI2C();
    msb = ReadI2C();
    IdleI2C();
    *_accel_Y = (256*msb) + lsb;
    AckI2C();
    lsb = ReadI2C();
    IdleI2C();
    AckI2C();
    IdleI2C();
    msb = ReadI2C();
    IdleI2C();
    *_accel_Z = (256*msb) + lsb;
    NotAckI2C();
    IdleI2C();
    StopI2C();
}

void ADXL345_GetBiass(float* _biass_X, float* _biass_Y, float* _biass_Z){

    int i;
    short accel_X, accel_Y, accel_Z;

    for (i = 0; i < ACC_SAMP_BIASS; i += 1) {
            ADXL345_read_data(&accel_X, &accel_Y, &accel_Z);
            *_biass_X += accel_X;
            *_biass_Y += accel_Y;
            *_biass_Z += accel_Z;
            MyMsDelay(1);
    }
    *_biass_X /= ACC_SAMP_BIASS;
    *_biass_Y /= ACC_SAMP_BIASS;
    *_biass_Z /= ACC_SAMP_BIASS;
    *_biass_Z -= 256;
}

Y el programa para testear el funcionamiento es el siguiente:

Código: [Seleccionar]

/*
 * File:   main.c
 * Author: elgarbe
 *
 * Created on 25 de abril de 2014, 12:40
 */
#define _XTAL_FREQ 8000000
#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <plib/i2c.h>
#include <plib/usart.h>
#include "ADXL345.h"
#include "configuracion_de_fuses.h"
#include "configuracion_hard.h"

#define ACEL_X_SCALE 0.0039 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.0039 //extraído del datasheet
#define ACEL_Z_SCALE 0.0039

void i2c_init(void);
void MyMsDelay(int ms);
// Variables globales.

void main(void) {
    short accel_X, accel_Y, accel_Z;
    float g_X, g_Y, g_Z;
    unsigned char MsgFromPIC[100];
    float Biass_X, Biass_Y, Biass_Z;

    // cambiamos la frecuencia del oscilador interno del valor por defecto de 32khz
    // a 8mhz mediante los bits del registro OSCCON
    OSCCONbits.IRCF2 = 1;
    OSCCONbits.IRCF1 = 1;
    OSCCONbits.IRCF0 = 1;

    ADCON1=0x0F; //Todos entrada/salida digitales. - equivalente en binario ADCON1= 0b00001111
    TRISA=0x00; //Todos como salida-   - equivalente en binario TRISA = 0b00000000
    TRISB=0X00; //Todos como salida.              - equivalente en binario TRISB = 0b00000000
    TRISC=0X00; //Todos como salida.              - equivalente en binario TRISC = 0b00000000
    TRISD=0x00;         //D7 a D1 como salida D0 como entrada
    TRISE=0X00; //Todos como salida.              - equivalente en binario TRISE = 0b00000000

    LATD=0x00; //Leds apagados.-
    TRISCbits.RC6 = 0; //TX pin set as output
    TRISCbits.RC7 = 1; //RX pin set as input

    LED1=1;
    MyMsDelay(5000);
    LED1=0;


    OpenUSART(USART_TX_INT_OFF &
        USART_RX_INT_ON &       //Activar la interrupcion por la recepcion de dato del buffer de Rx del USART
        USART_ASYNCH_MODE &     //Modo asincrono (fullduplex)
        USART_EIGHT_BIT &       //8 bits de datos
        USART_CONT_RX &         //Recepción continua
        USART_BRGH_HIGH, 51);   //9600 Baudios
   
    i2c_init();
    MyMsDelay(500);
    ADXL345_init();

    sprintf(MsgFromPIC, "Obteniendo Offset...\r\n");
    putsUSART(MsgFromPIC);
    ADXL345_GetBiass(&Biass_X, &Biass_Y, &Biass_Z);
    sprintf(MsgFromPIC, "Offsets: %.2f, %.2f, %.2f\r\n", Biass_X, Biass_Y, Biass_Z);
    putsUSART(MsgFromPIC);

    while(1){
        LED4=1;
        ADXL345_read_data(&accel_X, &accel_Y, &accel_Z);
        g_X = (accel_X - Biass_X) * ACEL_X_SCALE;
        g_Y = (accel_Y - Biass_Y) * ACEL_Y_SCALE;
        g_Z = (accel_Z - Biass_Z) * ACEL_Z_SCALE;
        sprintf(MsgFromPIC, "G's: %.2f, %.2f, %.2f\r\n", g_X, g_Y, g_Z);
//        sprintf(MsgFromPIC, "Offsets: %d, %d, %d\r\n", accel_X, accel_Y, accel_Z);
        putsUSART(MsgFromPIC);
        MyMsDelay(500);
        LED4=0;
        MyMsDelay(500);
    }
}

void i2c_init(void){
    /*A veces hay que hacer el siguiente work around explicado en el errata de
    microchip para asegurarse que no haya inconvenientes con el bus I2C
    Work around
    Before configuring the module for I2C operation:
    1. Configure the SCL and SDA pins as outputs by clearing their corresponding TRIS bits.
    2. Force SCL and SDA low by clearing the corresponding LAT bits.
    3. While keeping the LAT bits clear, configure SCL and SDA as inputs by setting their TRIS
    bits.
    */

    TRISBbits.TRISB0=0; //SET PINS AS OUTPUTS
    TRISBbits.TRISB1=0;
    PORTBbits.RB0 = 0;
    PORTBbits.RB1 = 0;
    TRISBbits.TRISB0=1; //SET PINS AS INPUTS
    TRISBbits.TRISB1=1;
    //////////////////////////////////////////////////Fin del Work Around////////////////////////////////////////////////////////////
    CloseI2C();
    OpenI2C(MASTER, SLEW_OFF);
    SSPADD = 19;    //100KHz con Xtal de 8MHz
    //I2C 400Khz  Los valores qeu funcionan para 100 Khz 129, para 400 Khz el valor es 29
    //La formula para el calculo es SSPADD = ((Fosc/BitRate)/4)-1
}

void MyMsDelay(int ms){
    while(ms--){
        __delay_ms(1);
    }
}

Primero con el sensor quieto tomamos 600 muestras y hacemos un promedio para obtener el Offset del sensor en cada eje.

Luego en el bucle principal leemos los datos RAW, le restamos el Offset y multiplicamos por la sensivilidad para obtener los datos directamente en G's.

Todo esto fue probado con la entrenadora. Ahora estoy trabajando en combinar el código de los post previos en uno solo y probarlos en la placa que fabrique el finde...

Saludos!

[BrunoF]

Hola Leo,

muy interesante todo lo que estás haciendo y especialmente, compartiendo.

Tengo una duda con respecto al algoritmo que estás usando para generar los PWM por sofware para los servos.

veo que hacés:

Código: C#

1. for(i=0;i<8;i++){   // de la salida correspondiente segun el duty elegido
2.                     dutys_buff[dutys[i]] |= mask;
3.                     mask <<= 1;
4.                 }

para invertir en el momento adecuado la salida de cada uno de los PWM, pero me  parece ver un bug en ese código. La OR siempre pone bits a 1, nunca a 0. Eso significa que el algoritmo está siempre introduciendo nuevos bits en 1 para los valores actuales de PWM, pero no veo que limpies los valores viejos cuando cambie el valor del PWM (dutys[ i ]) de un determinado motor.

Es decir, si por ejemplo, dutys[0] arranca valiendo 10, tu algoritmo va a poner a dutys_buff[10] al valor 0bxxxxxxx1. Donde x es desconocido porque técnicamente depende de los valores del resto de los otros 7 motores(dutys).

Hasta ahí perfecto. Pero si luego de cierto tiempo en ejecución, dutys[0] pasa a valer, por ejemplo, 20, ahora tu algoritmo pondrá a dutys_buff[20] al valor 0bxxxxxxx1.

Cuando se ejecute el próximo ciclo de PWM, el bit 0 del puerto asociado al PWM comenzará en 1 por lo que veo en tu algoritmo. Luego, en el paso 10, se pondrá a cero (debido a la XOR y a que dutys_buff[10] vale 0bxxxxxxx1. Se mantendrá en bajo hasta que el contador llegue a 20, durante el cual volverá a ponerse en alto y así quedará hasta que contador valga 101, momento en el cual forzas todas las salidas a bajo.

Obviamente esto se agrava (aparecerían más cambios de flancos erróneos) con cada valor distinto que va adquiriendo cada PWM de los motores.

Puedo estar equivocado, pero es lo que analizo al ver el código.  De optar por esa opción, vas a tener que proceder a limpiar el bit del elemento dutys_buff[?] viejo. Incluso me parece que generar las máscaras cada 20mS es innecesario, siendo que las máscaras sólo varían si varía algún valor de duty de algún motor.

Me parece que es mucho más sencillo sólo generar la máscara cuando se solicita un cambio de duty de un motor, e incluso allí aprovechar a limpiar el posible dutys_buff[?] viejo.

Ejemplo:

Código: C#

1. void set_pwm_duty(uint8_t pwmNum, uint8_t dutyValue)
2. {
3.         uint8_t mask = 1<<pwmNum;
4.  
5.         dutys_buff[dutys[pwmNum]]&= mask ^ 0xFF;     //limpiar bit de valor de duty previo. NO uso XOR acá porque fallaría en el primer cambio de valor de duty de cada motor.
6.         dutys_buff[dutyValue]|= mask;                          //introducir bit de valor de duty nuevo
7.         dutys[pwmNum] = dutyValue;                            //guardar nuevo valor de duty  
8. }

Saludos!

[elgarbe]

Bruno, excelente como siempre. Efectivamente se van acumulando los 1's...

Me parece muy buena la idea de la funcion set_pwm_duty, así que voy a implementarla.

Un saludo y gracias por revisar el código!!!!

[BrunoF]

Leo,

sigo pensando en el algorítmo y aún con lo que te dije, pueden calcularse mal algunos PWM. Si por algún motivo set_pwm_duty() se ejecuta durante un PWM en curso (supongamos que el generador de PWM va por el paso 50 de 100), y el valor viejo de PWM era, por ejemplo, 10 y el nuevo que se desea asignar es 70, al afectar al dutys_buff[70] y poner el bit correspondiente en 1, cuando el generador de PWM llegue al paso 70, va a pasar la salida a 1 nuevamente (debido a la XOR de tu algoritmo) y va a quedar en alto hasta el paso 101, en el cual forzás a cero las salidas. Digamos que introduce nuevamente otros flancos erróneos. Para solucionarlo definitivamente, sin tener que procesar más de lo actual podrías:

1) setear TODO el array dutys_buff[] a 0xFF (usando memset por ejemplo) por única vez al iniciar el programa.

2) cambiar

PORTD ^= dutys_buff[cont_int - 1];  // Si corresponde pongo a 0 la salida

por

PORTD &= dutys_buff[cont_int - 1];  // Si corresponde pongo a 0 la salida

3) cambiar la función set_pwm_duty()

a:

Código: C#

1. void set_pwm_duty(uint8_t pwmNum, uint8_t dutyValue)
2. {
3.         uint8_t mask = 1<<pwmNum;
4.  
5.         dutys_buff[dutys[pwmNum]]|= mask;                 //volver a 1 el bit del paso previo
6.         dutys_buff[dutyValue]&= mask ^ 255;                //poner a 0 el bit que queremos apagar en el paso elegido
7.         dutys[pwmNum] = dutyValue;                            //guardar nuevo valor de duty  
8. }

Te sobra bocho así que sé que me vas a seguir sin problemas.

Saludos!

[elgarbe]

Bruno, entiendo lo que me decis, el problema se origina por tomar un código y tratar de adaptarlo a las necesidades de uno si mucho estudio.
El código original (que cite en uno de los post: http://www.electro-tech-online.com/threads/drive-33-servos-with-one-pic-usart.34390/) era apoderarce de la interrupcion del timer y dentro de ella resolver el PWM de un grupo de salidas. Una vez que ese grupo habia finalizado, se sale de la interrupcion y se espera para procesar el siguiente grupo.
Yo muy feliz, tome parte de esas ideas, sin demaciado análisis evidentemente y lo adopte. Claro, el testeo que le hice es mínimo ya que recien estoy empezando con esto y en el apuro por avanzar meti la pata.

Los cambios sugeridos estan claros y parece que resolverán el problema de raíz. Este finde con tiempo voy a estar probando todo esto.
Una consulta, la sentencia "mask ^ 255" el lo mismo que invertir todos los bits de mask, verdad? es como el operador "~"?

El unico problema que veo es que si el duty actual es 80 y cuando voy por el paso 50 le asigno 40 como duty nuevo, durante esa pasada voy a tener la salida en 1 hasta el final. o sea que si quiero ir de un duty de 1.8mseg  a un duty de 1 mseg, puede darse un pulso intermedio de 2mseg. Quizá podría meter una mejora verificando si el duty nuevo es menor que el viejo. Si es así tendría que poner en cero la salida correspondiente... no se si se justifica el procesamiento adicional...

Saludos y gracias por las ideas!!!

[MGLSOFT]

No es un OR exclusivo ??