前言
上一節掌握了使用pwm驅動電機，接下來介紹如何使用msp432讀取mpu6050資料
正文
首先我們得知道mpu6050通訊方式，由於mpu6050只能用i2c通訊，所以學會使用msp432的i2c,msp432的i2c驅動可以呼叫driverlib庫來使用msp432的硬體i2c，但是i2c庫方法複雜使用起來會比較麻煩，
這裡我選擇偷個懶，用軟體方式模擬i2c驅動。
I2C
建立一個my_i2c.h
/* * my_i2c.h * * Created on: 2021年7月29日 * Author: Administrator */ #ifndef MY_I2C_H_ #define MY_I2C_H_ #include <ti/devices/msp432p4xx/driverlib/driverlib.h> #include <delay.h> #define SDA_IN() GPIO_setAsInputPin(GPIO_PORT_P6,GPIO_PIN4) #define SDA_OUT() GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN4) #define IIC_SCL_High() GPIO_setOutputHighOnPin(GPIO_PORT_P6,GPIO_PIN5) //SCL_High #define IIC_SCL_Low() GPIO_setOutputLowOnPin(GPIO_PORT_P6,GPIO_PIN5) //SCL_Low #define IIC_SDA_High() GPIO_setOutputHighOnPin(GPIO_PORT_P6,GPIO_PIN4) //SDA_High #define IIC_SDA_Low() GPIO_setOutputLowOnPin(GPIO_PORT_P6,GPIO_PIN4) //SDA_Low #define READ_SDA GPIO_getInputPinValue(GPIO_PORT_P6,GPIO_PIN4) //輸入SDA void IIC_Init(void); //初始化IIC的IO口 void IIC_Start(void); //傳送IIC開始訊號 void IIC_Stop(void); //傳送IIC停止訊號 void IIC_Send_Byte(uint8_t txd); //IIC傳送一個位元組 uint8_t IIC_Read_Byte(unsigned char ack);//IIC讀取一個位元組 uint8_t IIC_Wait_Ack(void); //IIC等待ACK訊號 void IIC_Ack(void); //IIC傳送ACK訊號 void IIC_NAck(void); //IIC不傳送ACK訊號 #endif /* MY_I2C_H_ */
my_i2c.c
/* * my_i2c.c * * Created on: 2021年7月29日 * Author: Administrator */ #include <my_i2c.h> void IIC_Init (void){ GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN5 ); //CLK GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN4);//DIN IIC_SCL_High(); IIC_SDA_High(); } void IIC_Start(void)//SDA 10 SCL 010 { SDA_OUT(); //sda線輸出 IIC_SCL_High(); IIC_SDA_High(); delay_us(4); IIC_SDA_Low();//START:when CLK is high,DATA change form high to low delay_us(4); IIC_SCL_Low();//鉗住I2C匯流排，準備傳送或接收資料 } void IIC_Stop(void)//SDA 01 SCL 01 { SDA_OUT();//sda線輸出 IIC_SCL_Low();//STOP:when CLK is high DATA change form low to high IIC_SDA_Low(); delay_us(4); IIC_SCL_High(); IIC_SDA_High();//傳送I2C匯流排結束訊號 delay_us(4); } //等待應答訊號到來 //返回值：1，接收應答失敗 // 0，接收應答成功 uint8_t IIC_Wait_Ack(void)// { uint8_t cy; SDA_IN(); //SDA設定為輸入 IIC_SCL_High();delay_us(10); IIC_SDA_High();delay_us(10); if(READ_SDA) { cy=1; IIC_SCL_Low(); return cy; } else { cy=0; } IIC_SCL_Low();//時鐘輸出0 return cy; } //產生ACK應答 void IIC_Ack(void) { IIC_SCL_Low(); SDA_OUT(); IIC_SDA_Low(); delay_us(2); IIC_SCL_High(); delay_us(2); IIC_SCL_Low(); } //不產生ACK應答 void IIC_NAck(void) { IIC_SCL_Low(); SDA_OUT(); IIC_SDA_High(); delay_us(2); IIC_SCL_High(); delay_us(2); IIC_SCL_Low(); } //IIC傳送一個位元組 //返回從機有無應答 //1，有應答 //0，無應答 void IIC_Send_Byte(uint8_t txd) { uint8_t t; SDA_OUT(); IIC_SCL_Low();//拉低時鐘開始資料傳輸 delay_us(2); for(t=0;t<8;t++) { if(txd&0x80) { IIC_SDA_High();delay_us(2); } else { IIC_SDA_Low();delay_us(2); } txd<<=1; IIC_SCL_High(); delay_us(4); IIC_SCL_Low(); delay_us(2); } delay_us(2); } //讀1個位元組，ack=1時，傳送ACK，ack=0，傳送nACK uint8_t IIC_Read_Byte(unsigned char ack) { unsigned char i,receive=0; SDA_IN();//SDA設定為輸入 for(i=0;i<8;i++ ) { IIC_SCL_Low(); delay_us(2); IIC_SCL_High(); receive<<=1; if(READ_SDA) receive++; delay_us(2); } if (!ack) IIC_NAck();//傳送nACK else IIC_Ack(); //傳送ACK return receive; }
好了到這裡i2c驅動有了，就可以繼續下一步了
MPU6050
mpu6050.h
/* * MPU6050.H * * Created on: 2021年7月29日 * Author: Administrator */ #ifndef MPU6050_H_ #define MPU6050_H_ #include <ti/devices/msp432p4xx/driverlib/driverlib.h> #include <stdint.h> #include <stdbool.h> #include <my_uart.h> #include <my_i2c.h> #include <delay.h> #include <math.h> #define MPU6050_ADDR 0x68 #define MPU6050_SMPLRT_DIV 0x19 #define MPU6050_CONFIG 0x1a #define MPU6050_GYRO_CONFIG 0x1b #define MPU6050_ACCEL_CONFIG 0x1c #define MPU6050_WHO_AM_I 0x75 #define MPU6050_PWR_MGMT_1 0x6b #define MPU6050_PWR_MGMT_2 0x6c #define MPU_ACCEL_XOUTH_REG 0x3b #define MPU_GYRO_XOUTH_REG 0x43 #define MPU6050_TEMP_H 0x41 #define MPU6050_TEMP_L 0x42 #define MPU_DEVICE_ID_REG 0x75 typedef uint8_t u8; typedef uint16_t u16; #define PI 3.1415926535897932384626433832795 extern int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ; extern float gyroXoffset, gyroYoffset, gyroZoffset; extern float temp, accX, accY, accZ, gyroX, gyroY, gyroZ; extern float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ; extern float angleX, angleY, angleZ; extern uint32_t timer; extern float accCoef; extern float gyroCoef; u8 MPU_Init(void); void calcGyroOffsets(void); void mpu_update(void); u8 MPU_Set_Gyro_Fsr(u8 fsr); u8 MPU_Set_Accel_Fsr(u8 fsr); u8 MPU_Set_LPF(u16 lpf); u8 MPU_Set_Rate(u16 rate); short MPU_Get_Temperature(); u8 MPU_Get_Gyroscope(int16_t *gx,int16_t *gy,int16_t *gz); u8 MPU_Get_Accelerometer(int16_t *ax,int16_t *ay,int16_t *az); u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf); u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); u8 MPU_Write_Byte(u8 reg,u8 data); u8 MPU_Read_Byte(u8 reg); #endif /* MPU6050_H_ */
mpu6050.c
/* * MPU6050.C * * Created on: 2021年7月29日 * Author: Administrator */ #include <MPU6050.H> int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ; float gyroXoffset, gyroYoffset, gyroZoffset; float temp, accX, accY, accZ, gyroX, gyroY, gyroZ; float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ; float angleX, angleY, angleZ; float gyroCoef,accCoef; uint32_t timer,preInterval; float interval; u8 MPU_Init(void) { accCoef = 0.02; gyroCoef = 0.98; IIC_Init(); MPU_Write_Byte(MPU6050_SMPLRT_DIV, 0x00); MPU_Write_Byte(MPU6050_CONFIG, 0x00); MPU_Write_Byte(MPU6050_GYRO_CONFIG, 0x08); MPU_Write_Byte(MPU6050_ACCEL_CONFIG, 0x00); MPU_Write_Byte(MPU6050_PWR_MGMT_1,0X01); mpu_update(); angleGyroX = 0; angleGyroY = 0; preInterval = timer; return 0; } void calcGyroOffsets(void){ float x = 0, y = 0, z = 0; int16_t rx, ry, rz; int i; for(i =0; i < 1000; i++){ MPU_Get_Gyroscope(&rx,&ry,&rz); x += ((float)rx) / 65.5; y += ((float)ry) / 65.5; z += ((float)rz) / 65.5; } gyroXoffset = x / 1000; gyroYoffset = y / 1000; gyroZoffset = z / 1000; } void mpu_update(void){ MPU_Get_Accelerometer(&rawAccX, &rawAccY, &rawAccZ); temp = MPU_Get_Temperature(); MPU_Get_Gyroscope(&rawGyroX,&rawGyroY,&rawGyroZ); accX = ((float)rawAccX) / 16384.0; accY = ((float)rawAccY) / 16384.0; accZ = ((float)rawAccZ) / 16384.0; angleAccX = atan2(accY, accZ + abs(accX)) * 360 / 2.0 / PI; angleAccY = atan2(accX, accZ + abs(accY)) * 360 / -2.0 / PI; gyroX = ((float)rawGyroX) / 65.5; gyroY = ((float)rawGyroY) / 65.5; gyroZ = ((float)rawGyroZ) / 65.5; gyroX -= gyroXoffset; gyroY -= gyroYoffset; gyroZ -= gyroZoffset; angleGyroX += gyroX * interval; angleGyroY += gyroY * interval; angleGyroZ += gyroZ * interval; interval = (timer - preInterval) * 0.001; angleX = (gyroCoef * (angleX + gyroX * interval)) + (accCoef * angleAccX); angleY = (gyroCoef * (angleY + gyroY * interval)) + (accCoef * angleAccY); angleZ = angleGyroZ; preInterval = timer ; } //設定MPU6050陀螺儀感測器滿量程範圍 //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps //返回值:0,設定成功 // 其他,設定失敗 u8 MPU_Set_Gyro_Fsr(u8 fsr) { return MPU_Write_Byte(MPU6050_GYRO_CONFIG,fsr<<3);//設定陀螺儀滿量程範圍 } //設定MPU6050加速度感測器滿量程範圍 //fsr:0,±2g;1,±4g;2,±8g;3,±16g //返回值:0,設定成功 // 其他,設定失敗 u8 MPU_Set_Accel_Fsr(u8 fsr) { return MPU_Write_Byte(MPU6050_ACCEL_CONFIG,fsr<<3);//設定加速度感測器滿量程範圍 } //設定MPU6050的數字低通濾波器 //lpf:數字低通濾波頻率(Hz) //返回值:0,設定成功 // 其他,設定失敗 u8 MPU_Set_LPF(u16 lpf) { u8 data=0; if(lpf>=188)data=1; else if(lpf>=98)data=2; else if(lpf>=42)data=3; else if(lpf>=20)data=4; else if(lpf>=10)data=5; else data=6; return MPU_Write_Byte(MPU6050_CONFIG,data);//設定數字低通濾波器 } //設定MPU6050的取樣率(假定Fs=1KHz) //rate:4~1000(Hz) //返回值:0,設定成功 // 其他,設定失敗 u8 MPU_Set_Rate(u16 rate) { u8 data; if(rate>1000)rate=1000; if(rate<4)rate=4; data=1000/rate-1; data=MPU_Write_Byte(MPU6050_SMPLRT_DIV,data); //設定數字低通濾波器 return MPU_Set_LPF(rate/2); //自動設定LPF為取樣率的一半 } //得到溫度值 //返回值:溫度值(擴大了100倍) short MPU_Get_Temperature(void) { u8 buf[2]; short raw; float temp; MPU_Read_Len(MPU6050_ADDR,MPU6050_TEMP_H,2,buf); raw=((u16)buf[0]<<8)|buf[1]; temp=36.53+((double)raw)/340; return temp*100;; } //得到陀螺儀值(原始值) //gx,gy,gz:陀螺儀x,y,z軸的原始讀數(帶符號) //返回值:0,成功 // 其他,錯誤程式碼 u8 MPU_Get_Gyroscope(int16_t *gx,int16_t *gy,int16_t *gz) { u8 buf[6],res; res=MPU_Read_Len(MPU6050_ADDR,MPU_GYRO_XOUTH_REG,6,buf); if(res==0) { *gx=((u16)buf[0]<<8)|buf[1]; *gy=((u16)buf[2]<<8)|buf[3]; *gz=((u16)buf[4]<<8)|buf[5]; } return res;; } //得到加速度值(原始值) //gx,gy,gz:陀螺儀x,y,z軸的原始讀數(帶符號) //返回值:0,成功 // 其他,錯誤程式碼 u8 MPU_Get_Accelerometer(int16_t *ax,int16_t *ay,int16_t *az) { u8 buf[6],res; res=MPU_Read_Len(MPU6050_ADDR ,MPU_ACCEL_XOUTH_REG,6,buf); if(res==0) { *ax=((u16)buf[0]<<8)|buf[1]; *ay=((u16)buf[2]<<8)|buf[3]; *az=((u16)buf[4]<<8)|buf[5]; } return res;; } //IIC連續寫 //addr:器件地址 //reg:暫存器地址 //len:寫入長度 //buf:資料區 //返回值:0,正常 // 其他,錯誤程式碼 u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf) { u8 i; IIC_Start(); IIC_Send_Byte((addr<<1)|0);//傳送器件地址+寫命令 if(IIC_Wait_Ack()) //等待應答 { IIC_Stop(); return 1; } IIC_Send_Byte(reg); //寫暫存器地址 IIC_Wait_Ack(); //等待應答 for(i=0;i<len;i++) { IIC_Send_Byte(buf[i]); //傳送資料 if(IIC_Wait_Ack()) //等待ACK { IIC_Stop(); return 1; } } IIC_Stop(); return 0; } //IIC連續讀 //addr:器件地址 //reg:要讀取的暫存器地址 //len:要讀取的長度 //buf:讀取到的資料儲存區 //返回值:0,正常 // 其他,錯誤程式碼 u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf) { IIC_Start(); IIC_Send_Byte((addr<<1)|0 );//傳送器件地址+寫命令 if(IIC_Wait_Ack()) //等待應答 { IIC_Stop(); return 1; } IIC_Send_Byte(reg); //寫暫存器地址 IIC_Wait_Ack(); //等待應答 IIC_Start(); IIC_Send_Byte((addr<<1)|1 );//傳送器件地址+讀命令 IIC_Wait_Ack(); //等待應答 while(len) { if(len==1)*buf=IIC_Read_Byte(0);//讀資料,傳送nACK else *buf=IIC_Read_Byte(1); //讀資料,傳送ACK len--; buf++; } IIC_Stop(); //產生一個停止條件 return 0; } //IIC寫一個位元組 //reg:暫存器地址 //data:資料 //返回值:0,正常 // 其他,錯誤程式碼 u8 MPU_Write_Byte(u8 reg,u8 data) { IIC_Start(); IIC_Send_Byte((MPU6050_ADDR << 1) | 0);//傳送器件地址+寫命令 if(IIC_Wait_Ack()) //等待應答 { IIC_Stop(); printf("%s\r\n","error"); delay_ms(100); return 1; } IIC_Send_Byte(reg); //寫暫存器地址 IIC_Wait_Ack(); //等待應答 IIC_Send_Byte(data);//傳送資料 if(IIC_Wait_Ack()) //等待ACK { IIC_Stop(); return 1; } IIC_Stop(); return 0; } //IIC讀一個位元組 //reg:暫存器地址 //返回值:讀到的資料 u8 MPU_Read_Byte(u8 reg) { u8 res; IIC_Start(); IIC_Send_Byte((MPU6050_ADDR << 1) | 0 );//傳送器件地址+寫命令 IIC_Wait_Ack(); IIC_Send_Byte(reg); //寫暫存器地址 IIC_Wait_Ack(); IIC_Start(); IIC_Send_Byte((MPU6050_ADDR << 1) | 1 );//傳送器件地址+讀命令 IIC_Wait_Ack(); res=IIC_Read_Byte(0);//讀取資料,傳送nACK IIC_Stop(); //產生一個停止條件 return res; }
定時器
由於姿態換算會用上積分，所以需要一個持續的時間值，我使用了一個增計數定時器
先配置定時器機構體
#define TIMER_PERIOD 2000 const Timer_A_UpModeConfig upConfig = { TIMER_A_CLOCKSOURCE_SMCLK, // SMCLK Clock Source TIMER_A_CLOCKSOURCE_DIVIDER_6, // SMCLK/1 = 3MHz TIMER_PERIOD, // 5000 tick period TIMER_A_TAIE_INTERRUPT_DISABLE, // Disable Timer interrupt TIMER_A_CCIE_CCR0_INTERRUPT_ENABLE , // Enable CCR0 interrupt TIMER_A_DO_CLEAR // Clear value };
定時器初始化
void Timer_init(void){ MAP_Timer_A_configureUpMode(TIMER_A1_BASE, &upConfig); MAP_Interrupt_enableInterrupt(INT_TA1_0); MAP_Timer_A_startCounter(TIMER_A1_BASE, TIMER_A_UP_MODE); Interrupt_enableMaster(); }
定時器中斷設定一個時間變數
void TA1_0_IRQHandler(void) { MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A1_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_0); timer++; }
中斷頻率為1ms,時間變數是32位，可以持續49天不會出錯
串列埠
為了檢查能否正常的讀取資料，使用串列埠列印到終端顯示
關於串列埠的使用，首先串列埠配置結構體
const eUSCI_UART_ConfigV1 uartConfig = { EUSCI_A_UART_CLOCKSOURCE_SMCLK, // SMCLK Clock Source 78, // BRDIV = 78 2, // UCxBRF = 2 0, // UCxBRS = 0 EUSCI_A_UART_NO_PARITY, // No Parity EUSCI_A_UART_LSB_FIRST, // LSB First EUSCI_A_UART_ONE_STOP_BIT, // One stop bit EUSCI_A_UART_MODE, // UART mode EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION, // Oversampling EUSCI_A_UART_8_BIT_LEN // 8 bit data length };
第一項時鐘源可以預設，但是實際沒有關係，因為一般使用dco時鐘作為串列埠時鐘，
第二項至第四項影響波特率關於其引數可以參考官方工具 http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html
在其後預設就好，串列埠初始化
void usart_init(void){ /* Selecting P1.2 and P1.3 in UART mode */ MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION); /* Setting DCO to 12MHz */ CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_12); //![Simple UART Example] /* Configuring UART Module */ MAP_UART_initModule(EUSCI_A0_BASE, &uartConfig); /* Enable UART module */ MAP_UART_enableModule(EUSCI_A0_BASE); /* Enabling interrupts */ MAP_UART_enableInterrupt(EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT); MAP_Interrupt_enableInterrupt(INT_EUSCIA0); MAP_Interrupt_enableMaster(); }
為方便使用重定義printf方法
int fputc(int _c, register FILE *_fp) { MAP_UART_transmitData( EUSCI_A0_BASE , (uint8_t) _c); return _c; } int fputs(const char *_ptr, register FILE *_fp) { uint16_t i, len; len = sizeof(_ptr); for(i=0; i<len; i++) { MAP_UART_transmitData( EUSCI_A0_BASE , (unsigned char)_ptr[i] ); } return len; }
my_uart.h
/* * my_uart.h * * Created on: 2021年7月29日 * Author: Administrator */ #ifndef MY_UART_H_ #define MY_UART_H_ #include <ti/devices/msp432p4xx/driverlib/driverlib.h> #include <stdio.h> #include <delay.h> void usart_init(); #endif /* MY_UART_H_ */
my_uart.c
/* * my_uart.c * * Created on: 2021年7月29日 * Author: Administrator */ #include <my_uart.h> int fputc(int _c, register FILE *_fp) { MAP_UART_transmitData( EUSCI_A0_BASE , (uint8_t) _c); return _c; } int fputs(const char *_ptr, register FILE *_fp) { uint16_t i, len; len = sizeof(_ptr); for(i=0; i<len; i++) { MAP_UART_transmitData( EUSCI_A0_BASE , (unsigned char)_ptr[i] ); } return len; } const eUSCI_UART_ConfigV1 uartConfig = { EUSCI_A_UART_CLOCKSOURCE_SMCLK, // SMCLK Clock Source 78, // BRDIV = 78 2, // UCxBRF = 2 0, // UCxBRS = 0 EUSCI_A_UART_NO_PARITY, // No Parity EUSCI_A_UART_LSB_FIRST, // LSB First EUSCI_A_UART_ONE_STOP_BIT, // One stop bit EUSCI_A_UART_MODE, // UART mode EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION, // Oversampling EUSCI_A_UART_8_BIT_LEN // 8 bit data length }; void usart_init(void){ /* Selecting P1.2 and P1.3 in UART mode */ MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION); /* Setting DCO to 12MHz */ CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_12); //![Simple UART Example] /* Configuring UART Module */ MAP_UART_initModule(EUSCI_A0_BASE, &uartConfig); /* Enable UART module */ MAP_UART_enableModule(EUSCI_A0_BASE); /* Enabling interrupts */ MAP_UART_enableInterrupt(EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT); MAP_Interrupt_enableInterrupt(INT_EUSCIA0); MAP_Interrupt_enableMaster(); } /* EUSCI A0 UART ISR - Echoes data back to PC host */ void EUSCIA0_IRQHandler(void) { uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A0_BASE); MAP_UART_clearInterruptFlag(EUSCI_A0_BASE, status); if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG) { MAP_UART_transmitData(EUSCI_A0_BASE, MAP_UART_receiveData(EUSCI_A0_BASE)); } }
資料檢測
mian.c
* Author: *******************************************************************************/ /* DriverLib Includes */ #include <ti/devices/msp432p4xx/driverlib/driverlib.h> /* Standard Includes */ #include <MPU6050.H> #include <my_PWM.H> #include <my_uart.h> #include <delay.h> #include <stdint.h> #include <stdbool.h> int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ; float gyroXoffset, gyroYoffset, gyroZoffset; float temp, accX, accY, accZ, gyroX, gyroY, gyroZ; float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ; float angleX, angleY, angleZ; uint32_t timer; double P[2][2] = {{ 1, 0 },{ 0, 1 }}; double Pdot[4] ={ 0,0,0,0}; static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.005,C_0 = 1; double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; #define TIMER_PERIOD 2000 const Timer_A_UpModeConfig upConfig = { TIMER_A_CLOCKSOURCE_SMCLK, // SMCLK Clock Source TIMER_A_CLOCKSOURCE_DIVIDER_6, // SMCLK/1 = 3MHz TIMER_PERIOD, // 5000 tick period TIMER_A_TAIE_INTERRUPT_DISABLE, // Disable Timer interrupt TIMER_A_CCIE_CCR0_INTERRUPT_ENABLE , // Enable CCR0 interrupt TIMER_A_DO_CLEAR // Clear value }; void Kalman_Filter(double angle_m,double gyro_m) { angleX+=(gyro_m-q_bias) * dtt; Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dtt; P[0][1] += Pdot[1] * dtt; P[1][0] += Pdot[2] * dtt; P[1][1] += Pdot[3] * dtt; angle_err = angle_m - angleX; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angleX+= K_0 * angle_err; q_bias += K_1 * angle_err; } void Timer_init(void){ MAP_Timer_A_configureUpMode(TIMER_A1_BASE, &upConfig); MAP_Interrupt_enableInterrupt(INT_TA1_0); MAP_Timer_A_startCounter(TIMER_A1_BASE, TIMER_A_UP_MODE); Interrupt_enableMaster(); } void LED_init(void){ MAP_GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_PIN0); MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P1, GPIO_PIN0); } int main(void) { MAP_WDT_A_holdTimer(); MAP_FPU_enableModule(); MAP_FPU_enableLazyStacking(); delay_init(12); usart_init(); MPU_Init(); Timer_init(); LED_init(); calcGyroOffsets(); GPIO_Motor_Init(); PWM_Init(); while(1){ mpu_update(); Kalman_Filter(angleX,gyroX ); printf(" %f %f %f %d %d \r\n ",angleX,mySetpoint,pwm,left_pulse_number,right_pulse_number); } } void TA1_0_IRQHandler(void) { MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A1_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_0); timer++; timer1++; if(timer1 == 1000){ timer1 = 0; GPIO_toggleOutputOnPin(GPIO_PORT_P1, GPIO_PIN0); } }