I’m an Engineering student from Lisbon, Portugal.I’m stuck with a school project, and my hope is that someone could help me.
I have been trying to program a Mini-imu v2.0
with a dspic30f4013, just the accelerometer and gyroscope. After
configure both of them i’m reading the data and sending them to a CAN
interface, so far so good. The problem is that both of the sensors are
sending me values of 0 or 65535. I have been trying to found the problem but
after review the code over and over again i just can find the error.
Here is my code:
(At this time i’m just trying to config the Gyro)
config.c:
/*
* File: config.c
* Author: Arreda
*
* Created on April 30, 2016, 5:40 PM
*/
#include <p30F4013.h>
#include "config.h"
#include "I2C.h"
#include "CAN.h"
#include "can_id.h"
#define I2C_WRITE 0
#define I2C_READ 1
#define ADDRESS_ACCEL 0x32 //00111100
#define ADDRESS_GYRO 0xD6 //11010011
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define CTRL_REG1_A 0x20
#define CTRL_REG4_A 0x23
void config_write(unsigned char regist, unsigned char data, unsigned char address){
I2C_Start();
I2C_Address(address,I2C_WRITE);
I2C_Write(regist);
I2C_Write(data);
I2C_Stop();
}
void confirm_write(unsigned char expected_value, unsigned char regist, unsigned char address){
unsigned char data_read;
int i = 0;
do{
config_write(regist, expected_value, address);
I2C_Start();
I2C_Address(address,I2C_WRITE);
I2C_Write(regist);
I2C_Restart();
I2C_Address(address,I2C_READ);
data_read = I2C_Read(0);
I2C_Stop();
/*i++;
if(i == 10){
CANdata erro_configuracao;
erro_configuracao.sid = CAN_ID_ERROR_CONF;
erro_configuracao.dlc = 8;
erro_configuracao.data[0] = 9;
erro_configuracao.data[1] = 9; //2 bytes compass data 1
erro_configuracao.data[1] = 9; //2 bytes
erro_configuracao.data[2] = 9; //2 bytes
CAN1_send(&erro_configuracao);
data_read = expected_value;
}**/
}while(data_read != expected_value);
}
void config(){
confirm_write(0x57,CTRL_REG1,ADDRESS_GYRO); //ODR = 190Hz, Cut-off 25Hz, Normal Mode
confirm_write(0x00,CTRL_REG4,ADDRESS_GYRO); // 250 dps
confirm_write(0x00,CTRL_REG4,ADDRESS_GYRO); //Normal mode, FIFO disabled
/* confirm_write(0x08,CTRL_REG4_A, ADDRESS_ACCEL); //
confirm_write(0x47,CTRL_REG1_A, ADDRESS_ACCEL); //ODR=100Hz, Cut-off 74 Hz*/
}
The code where i read the data:
/*
* File: read_imu.c
* Author: Arreda
*
* Created on May 2, 2016, 12:16 PM
*/
#include "read_imu.h"
#include "CAN.h"
#include "I2C.h"
#include"can_id.h"
#define ADDRESS_ACCEL 0x32 //00111100
#define ADDRESS_GYRO 0xD6 //11010011
#define I2C_WRITE 0
#define I2C_READ 1
#include <p30F4013.h>
int volatile executian_time;
void timer1_init(){
T1CONbits.TCKPS = 1; // Prescale 1:8
T1CONbits.TCS = 0; //usar internal clock Fcy/4
T1CONbits.TGATE = 0; //Nao ha acomulaçao
T1CONbits.TSIDL = 0; // Nao paro o timer no IDLE
TMR1 = 0; //valor inicial
PR1 = 9375; //value at which the register overflows ->valor maximo 2^16
//Valor para no final das contagens dar 10ms
//INTERRUPÇOES
IPC0bits.T1IP = 5; //define a prioridade da flag [0-7] // Prioridade do cpu é 3 tem de ser superior a isso ou nao vai ser chamada
IEC0bits.T1IE = 1; //enable da interrupçao
IFS0bits.T1IF = 0; //limpa a flag por segurança
void __attribute__(( interrupt, auto_psv,shadow)) _T1Interrupt(void){
executian_time ++; //incremento de 10ms
IFS0bits.T1IF = 0; // limpar a flag obrigatoriamente
return;
}
T1CONbits.TON = 1; //inicia o timer 1
}
void __attribute__(( interrupt, auto_psv,shadow)) _T1Interrupt(void){
executian_time ++; //incremento de 10ms
IFS0bits.T1IF = 0; // limpar a flag obrigatoriamente
return;
}
unsigned char get_data(unsigned char address, unsigned char register_address){
//Auxiliar fuction w/ the propose to read the data from the registers
//receiving as arg the address s chip, and the register from which will read the data, depending on the axes
unsigned char data;
I2C_Start();
I2C_Address(address, I2C_WRITE);
I2C_Write(register_address); //for aux = 0-> LSB
I2C_Restart(); //for aux = 1 ->MSB
I2C_Address(address, I2C_READ);
data=I2C_Read(0);
I2C_Stop();
return(data);
}
/*void read_accel(){
//Read data from accel
unsigned int x_accel, y_accel, z_accel;
unsigned char reg_ACC[6] = {0x28,0x29,0x2A,0x2B,0x2C,0x2D}; //0x28 and 0x29 X data
//0x2A and 0x2B Y data
unsigned char data_Xacc[2], data_Yacc[2], data_Zacc[2]; //data_x 0 is the LSB
//data_x 1 is the MSB
//Data 2 bytes
//cycle to acquire the LSB(i=0) and the MSB(i=1) of X,Y and Z
data_Xacc[0]= get_data(ADDRESS_ACCEL, reg_ACC[0]);
data_Xacc[1]= get_data(ADDRESS_ACCEL, reg_ACC[1]);
data_Yacc[0]= get_data(ADDRESS_ACCEL, reg_ACC[2]);
data_Yacc[1]= get_data(ADDRESS_ACCEL, reg_ACC[3]);
data_Zacc[0]= get_data(ADDRESS_ACCEL, reg_ACC[4]);
data_Zacc[1]= get_data(ADDRESS_ACCEL, reg_ACC[5]);
//concatenate the MSB and the LSB of X,Y and Z
//concatenation is made through a shiffer of 8 bits of the MSB
//To ensure that the additional 8 bits are zeros we use a AND with FF00
x_accel = ((data_Xacc[0]) | ((data_Xacc[1]<<8)&0xFF00));
y_accel = ((data_Yacc[0]) | ((data_Yacc[1]<<8)&0xFF00));
z_accel = ((data_Zacc[0]) | ((data_Zacc[1]<<8)&0xFF00));
CANdata CAN_ACCEL;
CAN_ACCEL.sid = CAN_ID_IMU_A;
CAN_ACCEL.dlc = 8;
CAN_ACCEL.data[0] = executian_time;
CAN_ACCEL.data[1] = x_accel; //2 bytes x accel
CAN_ACCEL.data[2] = y_accel; //2 bytes
CAN_ACCEL.data[3] = z_accel; //2 bytes
CAN1_send(&CAN_ACCEL);
}*/
void read_gyro(){
//Read data from accel
unsigned int x_gyro, y_gyro, z_gyro;
unsigned char reg_GYRO[6] = {0x28,0x29,0x2A,0x2B,0x2C,0x2D}; //0x28 and 0x29 X data
//0x2A and 0x2B Y data
unsigned char data_Xgyro[2], data_Ygyro[2], data_Zgyro[2]; //data_x 0 is the LSB
//data_x 1 is the MSB
//Data 2 bytes
//cycle to acquire the LSB(i=0) and the MSB(i=1) of X,Y and Z
data_Xgyro[0]= get_data(ADDRESS_GYRO, reg_GYRO[0]);
data_Xgyro[1]= get_data(ADDRESS_GYRO, reg_GYRO[1]);
data_Ygyro[0]= get_data(ADDRESS_GYRO, reg_GYRO[2]);
data_Ygyro[1]= get_data(ADDRESS_GYRO, reg_GYRO[3]);
data_Zgyro[0]= get_data(ADDRESS_GYRO, reg_GYRO[4]);
data_Zgyro[1]= get_data(ADDRESS_GYRO, reg_GYRO[5]);
//concatenate the MSB and the LSB of X,Y and Z
//concatenation is made through a shiffer of 8 bits of the MSB
//To ensure that the additional 8 bits are zeros we use a AND with FF00
x_gyro = ((data_Xgyro[0]) | ((data_Xgyro[1]<<8)&0xFF00));
y_gyro = ((data_Ygyro[0]) | ((data_Ygyro[1]<<8)&0xFF00));
z_gyro = ((data_Zgyro[0]) | ((data_Zgyro[1]<<8)&0xFF00));
CANdata CAN_GYRO;
CAN_GYRO.sid = CAN_ID_IMU_G;
CAN_GYRO.dlc = 8;
CAN_GYRO.data[0] = executian_time;
CAN_GYRO.data[1] = x_gyro; //2 bytes x accel
CAN_GYRO.data[2] = y_gyro; //2 bytes
CAN_GYRO.data[3] = z_gyro; //2 bytes
CAN1_send(&CAN_GYRO);
}
void read_temp(){
//Read data from accel
unsigned char reg_temp= 0x26;
unsigned char data_temp; //data_x 0 is the LSB
//data_x 1 is the MSB
//Data 2 bytes
data_temp = get_data(ADDRESS_GYRO, reg_temp);
CANdata CAN_temp;
CAN_temp.sid = CAN_ID_TEMP;
CAN_temp.dlc = 8;
CAN_temp.data[0] =executian_time;
CAN_temp.data[1] =reg_temp; //2 bytes x accel
CAN_temp.data[2] = 0;
CAN1_send(&CAN_temp);
}
#include <p30F4013.h>
#include "I2C.h"
void I2C_config(){
I2CCONbits.I2CSIDL = 1; // Discontinue in idle mode
I2CCONbits.IPMIEN = 0; // IPMI mode disabled
I2CCONbits.DISSLW = 0; // Slew rate control disabled
I2CCONbits.SMEN = 0; // Disable SMBus input thresholds
I2CCONbits.GCEN = 0; // General call address disbled
I2CBRG = 67; // Fcy 7.5Mhz Fscl = 100kHz
I2CCONbits.I2CEN = 1; // I2C Enable bit - Enabled
}
void I2C_Wait(void){
while(I2CCONbits.SEN || I2CCONbits.PEN || I2CCONbits.RCEN ||
I2CCONbits.ACKEN || I2CSTATbits.TRSTAT || I2CCONbits.RSEN); /* esperar que nao esteja nenhuma operaçao esteja a ser efectuada*/
}
// i2c_Start - Start I2C communication
void I2C_Start(void){
I2C_Wait();
I2CCONbits.SEN = 1; // Start Condition
}
// i2c_Restart - Re-Start I2C communication
void I2C_Restart(void){
I2C_Wait();
I2CCONbits.RSEN = 1; // Re-start Condition
}
// i2c_Stop - Stop I2C communication
void I2C_Stop(void){
I2C_Wait();
I2CCONbits.PEN=1;
}
// i2c_Write - Sends one byte of data
void I2C_Write(unsigned char data){
I2C_Wait();
I2CTRN = data;
while(I2CSTATbits.TBF); // While transmit not complete, I2CTRN is full
}
// i2c_Address - Sends Slave Address and Read/Write mode
// mode is either I2C_WRITE or I2C_READ
void I2C_Address(unsigned char address, unsigned char mode){
unsigned char s_address;
s_address=address<<1;
s_address+=mode;
I2C_Wait();
I2CTRN = s_address;
while(I2CSTATbits.TBF); // While transmit not complete, I2CTRN is full
}
// i2c_Read - Reads a byte from Slave device
unsigned char I2C_Read(unsigned char ack){
// Read data from slave
// ack should be 1 if there is going to be more data read
// ack should be 0 if this is the last byte of data read
unsigned char I2CReadData;
I2C_Wait();
I2CCONbits.RCEN = 1; // Recieve sequence in progress
I2C_Wait();
I2CReadData = I2CRCV;
I2C_Wait();
if (ack) I2CCONbits.ACKDT=0; // Send ACK during acknowledge
else I2CCONbits.ACKDT=1; // NAck
I2CCONbits.ACKEN = 1; // Acknowledge sequence in progress 0
return(I2CReadData);
}
and the main:
#include <stdio.h>
#include <stdlib.h>
#include "read_imu.h"
#include "CAN.h"
#include "config.h"
#include "I2C.h"
#include "can_id.h"
#include <p30F4013.h>
//Configuration bits
_FOSC ( CSW_FSCM_OFF & HS2_PLL4 );
_FWDT ( WDT_OFF );
_FBORPOR( PBOR_OFF & PWRT_16 & MCLR_EN );
_FGS ( CODE_PROT_OFF );
_FICD ( PGD );
#define FOSC (30000000ULL) ; //30MHz Cristal
void timer2_init(void){
T2CONbits.TCS = 0; // internal clock (Fcy/4)
T2CONbits.TCKPS = 1; //prescale 1:8
T2CONbits.TGATE = 0; // timer gated time accumulation disabled
T2CONbits.TSIDL = 0; // continue timer in idle mode
TMR2 = 0; //initial value
PR2 = 9375; //final time to make a 'cycle' of 10ms
/*interrupts*/
IPC1bits.T2IP = 5; //priority level
IFS0bits.T2IF = 0; //clean the flag only for security
IEC0bits.T2IE = 1; //Interrupt enable
T2CONbits.TON = 1; // Inicia o timer 2
return;
}
//Timer 2 interrupt
void __attribute__(( interrupt, auto_psv,shadow)) _T2Interrupt(void){
//read_accel();
read_gyro();
read_temp();
IFS0bits.T2IF = 0; // limpar a flag obrigatoriamente
return;
}
int main(){
CAN1_config();
I2C_config();
timer2_init();
config();
timer1_init();
TRISDbits.TRISD0=0;
CANdata reset_message;
reset_message.sid = CAN_ID_MODULE_RESET;
reset_message.dlc = 8;
reset_message.data[0] = 0;
reset_message.data[1] = CAN_ID_IMU_A; //2 bytes compass data 1
reset_message.data[1] = 0; //2 bytes
reset_message.data[2] = 0; //2 bytes
CAN1_send(&reset_message);
CAN1_send(&reset_message);
IEC0bits.T2IE = 1; //Interrupt enable timer 2
while(1){
// Idle();
}
return(EXIT_SUCCESS);
}
Thanks