he logrado grabar el programa en el dspic, el magnetometro funciona y responde los datos de orientacion con respecto al norte.
el problema surge al apagar el dspic, al volver a prenderlo me dan valores estancados, sin variacion alguna, vuelvo a grabar el dspic y vuelve a funcionar correctamente, no logro entender donde esta el problema.
aqui esta el codigo:
#include "xc.h"
#include <libpic30.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <p33Exxxx.h>
#define MIPS 70 //
#define crystal 16 //Mhz
#define FCY MIPS*1000000
#define BAUDRATE_TABLET 38400
#define BRGVAL_TABLET ((FCY/BAUDRATE_TABLET)/16)-1
#define RAD_A_DEG 57.295779
#define PI 3.14159265359
_FOSCSEL(FNOSC_PRIPLL);
//// Enable clock switching and configure
////_FOSC(FCKSM_CSECMD & OSCIOFNC_OFF); //interno oscilador
_FOSC(FCKSM_CSECMD & IOL1WAY_OFF & OSCIOFNC_OFF & POSCMD_HS);
_FWDT(FWDTEN_OFF & WINDIS_OFF & WDTPRE_PR128 & WDTPOST_PS32768); // Watchdog des-habilitado.
_FICD(JTAGEN_OFF & ICS_PGD1);
// FICD
#define delay_ms(x) __delay32((FCY/1000)*x)
#define delay_us(x) __delay32(MIPS*x)
void config_io_digital(void);
void config_clk_HSPLL(void);
void configuracion_UART_TABLET(void);
void lectura_HMC5883L(void);
void config_I2C_HMC5883L(void);
void data_magnetometer(void);
void iniciar_I2C(void);
void finalizar_I2C(void);
int escribir_I2C(int DirecEsclavo, int DirecRegistro, int dato);
int lectura_I2C(int DirecEsclavo, int DirecRegistro);
void lectura_i2c_rafaga(int DirecEsclavo, int DirecRegistro, int cantidad_registros);
void config_I2C(void);
float correctAngle(float heading);
void configuracion_timer3(void);
int i;
char Trama_Magnetometer[16];
float heading1 = 1.0; //dato final respecto al norte
float MagData[3];
int estado_i2c = 0;
int i2c_rafaga_dato[16];
float xMagnetFiltered = 0;
float yMagnetFiltered = 0;
float zMagnetFiltered = 0;
float xMagnetFilteredOld = 0;
float yMagnetFilteredOld = 0;
float zMagnetFilteredOld = 0;
float alphaMagnet = 0.099;
int cont;
int main(void) {
_TRISA9 = 0;
_LATA9 = 0;
config_io_digital();
config_clk_HSPLL();
config_I2C();
config_I2C_HMC5883L();
configuracion_UART_TABLET(); //U3
//
while (1) {
_LATA9 = ~_LATA9;
lectura_HMC5883L();
data_magnetometer();
sprintf(Trama_Magnetometer, "%.3f \n", (double) heading1);
for (cont = 0; cont < strlen(Trama_Magnetometer); cont++) {
while (U3STAbits.UTXBF);
U3TXREG = Trama_Magnetometer[cont];
while (!U3STAbits.TRMT);
}
delay_ms(250);
}
return 0;
}
void configuracion_UART_TABLET(void) {
//Configurar COMO TRASNMISION RP55
RPOR6bits.RP55R = 27;
//Configurar COMO RECEPCION RP54
RPINR27bits.U3RXR = 0x36; //
////******Configuracion del modulo uart
U3MODEbits.STSEL = 0; // 1 Stop bit
U3MODEbits.PDSEL = 0; // No Parity, 8 data bits
U3MODEbits.ABAUD = 0; // Auto-Baud Disabled
U3MODEbits.BRGH = 0; // Low-Speed mode
U3BRG = BRGVAL_TABLET; // BAUD Rate Setting for 38400
U3STAbits.URXISEL = 0; // Interrupt after one RX character is received
IFS5bits.U3RXIF = 0; // Clear the Recieve Interrupt Flag
IEC5bits.U3RXIE = 1; // Enable Recieve Interrupts
U3MODEbits.UARTEN = 1; // Enable UART
U3STAbits.UTXEN = 1; // Enable UART TX
}
void lectura_HMC5883L(void) {
iniciar_I2C();
lectura_i2c_rafaga(0x3C, 0x03, 6); //no es necesario comprobar que este listo para leer
////
finalizar_I2C();
MagData[0] = ((i2c_rafaga_dato[0] << 8 ) | i2c_rafaga_dato[1])*0.92;
MagData[1] = ((i2c_rafaga_dato[4] << 8 ) | i2c_rafaga_dato[5])*0.92;
MagData[2] = ((i2c_rafaga_dato[2] << 8 ) | i2c_rafaga_dato[3])*0.92;
}
void data_magnetometer(void) {
/********filtro pasabajo magnetometro*/
xMagnetFiltered = xMagnetFiltered + alphaMagnet * (MagData[0] - xMagnetFiltered);
yMagnetFiltered = yMagnetFiltered + alphaMagnet * (MagData[1] - yMagnetFiltered);
zMagnetFiltered = zMagnetFiltered + alphaMagnet * (MagData[2] - zMagnetFiltered);
heading1 = atan2(yMagnetFiltered, xMagnetFiltered);
float declinationAngle = (-1.0 + (28.0 / 60.0)) / (RAD_A_DEG);
heading1 += declinationAngle;
//heading2 += declinationAngle;
// Correct for heading < 0deg and heading > 360deg
heading1 = RAD_A_DEG * correctAngle(heading1);
//heading2 = RAD_A_DEG * correctAngle(heading2);
}
void config_I2C_HMC5883L(void) {
//PARA EL MAGNETOMETRO HMC5883L
iniciar_I2C();
estado_i2c = escribir_I2C(0x3C, 0x02, 0x00); //activar magnetometro ok
finalizar_I2C();
__delay32(800000);
iniciar_I2C();
estado_i2c = escribir_I2C(0x3C, 0x01, 0x01); //reg B---sacle 0.92
finalizar_I2C();
__delay32(800000);
}
float correctAngle(float heading) {
if (heading < 0) {
heading += 2 * PI;
}
return heading;
}
void iniciar_I2C(void) {
//Esperar a que se desocupe I2C
while (I2C1CONbits.SEN || I2C1CONbits.PEN || I2C1CONbits.RCEN || I2C1CONbits.ACKEN || I2C1STATbits.TRSTAT);
//Comienzo la comunicacion I2c
I2C1CONbits.SEN = 1;
//Esperamos a que la secuencia sea finalizada
while (I2C1CONbits.SEN);
//Limpia el flag de interrupcion
IFS1bits.MI2C1IF = 0;
}
void finalizar_I2C(void) {
while (I2C1CONbits.SEN || I2C1CONbits.PEN || I2C1CONbits.RCEN || I2C1CONbits.ACKEN || I2C1STATbits.TRSTAT);
//detiene i2c
I2C1CONbits.PEN = 1;
//Espera hasta que secuencia stop sea finalizada
while (I2C1CONbits.PEN);
}
int escribir_I2C(int DirecEsclavo, int DirecRegistro, int dato) {
//Escribe la direccion del esclavo y activa el maestro para la transmision.
I2C1TRN = DirecEsclavo; //0xE;//DIRECCION DEL SCLAVO -- mpu6050 1101000x -- x escribir o leer.
//para leer del mpu 1101 0000 =0xD0 -- direccion del esclavo e instruccion de escribir
if (I2C1STATbits.IWCOL) { //Si ocurre una colision retornar -1
return -1;
}
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Escribo direccion al cual quiero escribir
I2C1TRN = DirecRegistro; //registro donde se encuentra la direccion del esclavo
if (I2C1STATbits.IWCOL) //Si ocurre una colision retornar -1
return -1;
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Escribo el dato
I2C1TRN = dato; //registro donde se encuentra la direccion del esclavo
if (I2C1STATbits.IWCOL) //Si ocurre una colision retornar -1
return -1;
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
// ///********************************************
return 1;
}
int lectura_I2C(int DirecEsclavo, int DirecRegistro) {
//PORTA=1;
//Escribe la direccion del esclavo y activa el maestro para la transmision.
I2C1TRN = DirecEsclavo; //0xE;//DIRECCION DEL SCLAVO -- mpu6050 1101000x -- x escribir o leer.
//para leer del mpu 1101 0000 =0xD0 -- direccion del esclavo e instruccion de escribir
if (I2C1STATbits.IWCOL) { //Si ocurre una colision retornar -1
return -1;
}
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Escribo direccion al cual quiero leeer
I2C1TRN = DirecRegistro; //registro donde se encuentra la direccion del esclavo
if (I2C1STATbits.IWCOL) //Si ocurre una colision retornar -1
return -1;
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Envia comenzar otra vez el i2c.
//Comienzo la comunicacion I2c
I2C1CONbits.RSEN = 1;
//Esperamos a que la secuencia sea finalizada
while (I2C1CONbits.RSEN);
//Limpia el flag de interrupcion
IFS1bits.MI2C1IF = 0;
//Escribe la direccion del esclavo y activa el maestro para la transmision.
I2C1TRN = DirecEsclavo + 1; //0xE;//DIRECCION DEL SCLAVO -- mpu6050 1101000x -- x escribir o leer.
//para leer del mpu 1101 0001 =0xD1 -- direccion del esclavo e instruccion de leer
if (I2C1STATbits.IWCOL) //Si ocurre una colision retornar -1
return -1;
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Espero hasta que dato sea recepcionado
//Recibe dato del mpu
I2C1CONbits.RCEN = 1; //habilita recepcion de i2c
while (I2C1CONbits.RCEN); //Espera a que recepcion ya no este en proceso
I2C1STATbits.I2COV = 0;
//while(!I2C1STATbits.RBF);
return (I2C1RCV);
}
void lectura_i2c_rafaga(int DirecEsclavo, int DirecRegistro, int cantidad_registros) {
//PORTA=1;
//Escribe la direccion del esclavo y activa el maestro para la transmision.
I2C1TRN = DirecEsclavo; //0xE;//DIRECCION DEL SCLAVO -- mpu6050 1101000x -- x escribir o leer.
//para leer del mpu 1101 0000 =0xD0 -- direccion del esclavo e instruccion de escribir
//PORTAbits.RA0=0;++++++++++ PASO TEST
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Escribo direccion al cual quiero leeer
I2C1TRN = DirecRegistro; //registro donde se encuentra la direccion del esclavo
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Comienzo la comunicacion I2c
I2C1CONbits.RSEN = 1;
//Esperamos a que la secuencia sea finalizada
while (I2C1CONbits.RSEN);
//Limpia el flag de interrupcion
IFS1bits.MI2C1IF = 0;
//Escribe la direccion del esclavo y activa el maestro para la transmision.
I2C1TRN = DirecEsclavo + 1; //0xE;//DIRECCION DEL SCLAVO -- mpu6050 1101000x -- x escribir o leer.
//para leer del mpu 1101 0001 =0xD1 -- direccion del esclavo e instruccion de leer
//Esperar mientras transmision esta en progreso
while (I2C1STATbits.TBF); // 8 ciclos de reloj, 7 de direccion y 1 de modo.
while (!IFS1bits.MI2C1IF); //Espera interrupcion
IFS1bits.MI2C1IF = 0; // limpia el flag
while (I2C1STATbits.ACKSTAT); // Espera el ACK del esclavo.
//Espero hasta que dato sea recepcionado
//Recibe dato del mpu
for (i = 0; i <= (cantidad_registros - 1); i++) {
I2C1CONbits.RCEN = 1; //habilita recepcion de i2c
while (I2C1CONbits.RCEN); //Espera a que recepcion ya no este en proceso
I2C1STATbits.I2COV = 0;
i2c_rafaga_dato
= I2C1RCV;
if (i == (cantidad_registros - 1))
break;
I2C1CONbits.ACKDT = 0;
I2C1CONbits.ACKEN = 1;
while (I2C1CONbits.ACKEN);
}
I2C1CONbits.ACKDT = 1;
I2C1CONbits.ACKEN = 1;
}
void config_I2C(void) {
///cambiar de acuerdo a nuevas patitas del nuevo dspic, no lo cambie porque no lo necesito
// CNPU2bits.CN21PUE = 1;//Lo comento ya que la tarjetita ya tiene resistencias pull-up
// CNPU2bits.CN22PUE = 1;
//I2C1BRG = 0x00A3;//163.9 for speed i2c of 400Khz to FCY = 70Mhz
I2C1BRG = 0x00A3; //163.9 for speed i2c of 400Khz to FCY = 70Mhz
I2C1CON = 0xC220;
}
void configuracion_timer3(void) {//magnetometer
//Habilito interrupcion del timer y lo coloco como prioridad 1
float tmili = 50; /*variable a modficar en ms*/
float preescaler = 100.0;
T3CON = 0; // Borrar la configuración del temporizador 1
TMR3 = 0; // Reset Timer 1 counter
T3CONbits.TCKPS = 3; // Precontador (0 = 1:1, 1 = 1:8, 2 = 1:64, 3 = 1:256)
switch (T3CONbits.TCKPS) {
case 0: preescaler = 1.0;
break;
case 1:preescaler = 8.0;
break;
case 2:preescaler = 64.0;
break;
case 3:preescaler = 256.0;
break;
default:break;
}
PR3 = 1 * (int) (((float) (FCY) / preescaler)*0.001 * tmili);
// PR1 = 781.25; // Establecer el Intervalo del temporizador 1 (máximo 65.535)
//PR2 = 273 * (unsigned int)tmili;
T3CONbits.TCS = 0; // Seleccionar el reloj interno (Fosc / 4)
_T3IP = 1; // Establecer el temporizador 1 prioridad de interrupción
_T3IF = 0; // Borrar la bandera de interrupción del temporizador 1
_T3IE = 1; // Habilitar el temporizador 1 de interrupción
T3CONbits.TON = 1; // Activar el temporizador 1
while (IFS0bits.T3IF == 0);
}
void config_io_digital(void) {
ANSELA = 0x00;
ANSELB = 0x00;
ANSELC = 0x00;
}
void config_clk_HSPLL(void) {//con oscilador externo
int M = MIPS * 8 / crystal;
PLLFBD = M - 2; // M = 28
CLKDIVbits.PLLPRE = 0; // N1 = 2
CLKDIVbits.PLLPOST = 0; // N2 = 2
// Initiate Clock Switch to Primary Oscillator with PLL (NOSC = 0b011)
__builtin_write_OSCCONL(0x01);
__builtin_write_OSCCONH(0x03);
// Wait for Clock switch to occur
while (OSCCONbits.COSC != 0b011) {
__delay32(1000000);
};
while (OSCCONbits.LOCK != 1) {
};
}
