2020年07月29日 | mpu6050六轴传感器msp430驱动程序

#include

#include"mpu6050.h"

#include "USCI_A0.h"

#include "IMU.h "

/*

*  ======== BCSplus_graceInit ========

*  Initialize MSP430 Basic Clock System

*/

void BCSplus_graceInit(void)

{

    /* USER CODE START (section: BCSplus_graceInit_prologue) */

    /* User initialization code */

    /* USER CODE END (section: BCSplus_graceInit_prologue) */

    /*

     * Basic Clock System Control 2

     *

     * SELM_0 -- DCOCLK

     * DIVM_0 -- Divide by 1

     * ~SELS -- DCOCLK

     * DIVS_1 -- Divide by 2

     * ~DCOR -- DCO uses internal resistor

     *

     * Note: ~ indicates that has value zero

     */

    BCSCTL2 = SELM_0 | DIVM_0 | DIVS_1;

    if (CALBC1_16MHZ != 0xFF) {

        /* Adjust this accordingly to your VCC rise time */

        __delay_cycles(100000);

        /* Follow recommended flow. First, clear all DCOx and MODx bits. Then

         * apply new RSELx values. Finally, apply new DCOx and MODx bit values.

         */

        DCOCTL = 0x00;

        BCSCTL1 = CALBC1_16MHZ;     /* Set DCO to 16MHz */

        DCOCTL = CALDCO_16MHZ;

    }

    /*

     * Basic Clock System Control 1

     *

     * XT2OFF -- Disable XT2CLK

     * ~XTS -- Low Frequency

     * DIVA_0 -- Divide by 1

     *

     * Note: ~XTS indicates that XTS has value zero

     */

    BCSCTL1 |= XT2OFF | DIVA_0;

    /*

     * Basic Clock System Control 3

     *

     * XT2S_0 -- 0.4 - 1 MHz

     * LFXT1S_0 -- If XTS = 0, XT1 = 32768kHz Crystal ; If XTS = 1, XT1 = 0.4 - 1-MHz crystal or resonator

     * XCAP_1 -- ~6 pF

     */

    BCSCTL3 = XT2S_0 | LFXT1S_0 | XCAP_1;

    /* USER CODE START (section: BCSplus_graceInit_epilogue) */

    /* User code */

    /* USER CODE END (section: BCSplus_graceInit_epilogue) */

}/*

* main.c

*/

int main(void)

{

    WDTCTL = WDTPW | WDTHOLD;        // Stop watchdog timer

    BCSplus_graceInit();        //MCLK = 16M,SMCLK = 8M,ack =32.768k

    WDTCTL = WDTPW | WDTTMSEL | WDTIS0;

    IE1 |= WDTIE;

    USCI_A0_init();             //uart 38400bps  

    P1DIR |= BIT0;              //initialize led control PIN

    _EINT();

    MPU6050_Init();             //initialize mpu6050

    while(1)

    {

      /*

      Get_Attitude();

     //MPU6050_Dataanl();

     // ReadMPU6050All();

     //Prepare_Data();             //耗时6.15ms

     //Uart1_Send_AF();             //耗时8.42ms

     P1OUT ^= BIT0;

     Get_Attitude();

     //ReadMPU6050All();

     //MPU6050_Dataanl();         //未使用多字节读取，耗时6ms,使用多字节读取，耗时3.6ms

     //Prepare_Data();

    // Uart1_Send_AF();

     P1OUT ^= BIT0;

     */

    }

}

//watchdog interrupt server program per 1ms

#pragma vector=WDT_VECTOR

__interrupt void WDT_ISR_HOOK(void)

{

    /* USER CODE START (section: WDT_ISR_HOOK) */

    /* replace this comment with your code */

    static unsigned int count=0;

    static unsigned char ms2 = 0,ms5 = 0,ms10 = 0;

    count++;

    ms2++;

    ms5++;

    ms10++;

    if(ms2 == 2)

    {

     ms2 = 0;

     Prepare_Data();

     //MPU6050_Dataanl();

    }

    if(ms5 >= 4)

    {

      ms5 = 0;

      Get_Attitude();

    }

    if(ms10 >= 10)

    {

      ms10 = 0;

      Uart1_Send_AF();

    }

    if(count== 200)

    {

      count=0;

      P1OUT ^= BIT0;

    }

        /* USER CODE END (section: WDT_ISR_HOOK) */

}

/*

* This file contains some mpu6050 operation.

* By IC爬虫 (1394024051@qq.com)

* 2014-4-13 v1.0

*/

#include "mpu6050.h"

unsigned char   mpu6050_buffer[14];     //I2C读取后存放数据

int ACC_OFFSET_X,ACC_OFFSET_Y,ACC_OFFSET_Z;

int GYRO_OFFSET_X,GYRO_OFFSET_Y,GYRO_OFFSET_Z;

unsigned char        GYRO_OFFSET_OK = 1;

unsigned char        ACC_OFFSET_OK = 1;

int MPU6050_ACC_LAST_X,MPU6050_ACC_LAST_Y,MPU6050_ACC_LAST_Z;    //final accelerate speed

int MPU6050_GYRO_LAST_X,MPU6050_GYRO_LAST_Y,MPU6050_GYRO_LAST_Z; //final  gyro  speed

/**********************************************************/

//函数名称：void MPU6050_Dataanl

//入口参数：无

//出口参数：无

//函数功能：MPU6050数据读取并处理

/**********************************************************/

void MPU6050_Dataanl(void)

{

#ifndef READALL

  MPU6050_ACC_LAST_X = GetAccelX() - ACC_OFFSET_X;

  MPU6050_ACC_LAST_Y = GetAccelY() - ACC_OFFSET_Y;

  MPU6050_ACC_LAST_Z = GetAccelZ() - ACC_OFFSET_Z;

  MPU6050_GYRO_LAST_X = GetAnguX() - GYRO_OFFSET_X;

  MPU6050_GYRO_LAST_Y = GetAnguY() - GYRO_OFFSET_Y;

  MPU6050_GYRO_LAST_Z = GetAnguZ() - GYRO_OFFSET_Z;

  //------------------------------------------------------------------//

  //补偿偏移

  if(!GYRO_OFFSET_OK)

  {

    static long int tempgx=0,tempgy=0,tempgz=0;

    static unsigned char cnt_g=0;

    if(cnt_g==0)

    {

      GYRO_OFFSET_X=0;

      GYRO_OFFSET_Y=0;

      GYRO_OFFSET_Z=0;

      tempgx = 0;

      tempgy = 0;

      tempgz = 0;

      cnt_g = 1;

    }

    tempgx+= MPU6050_GYRO_LAST_X;

    tempgy+= MPU6050_GYRO_LAST_Y;

    tempgz+= MPU6050_GYRO_LAST_Z;

    if(cnt_g==200)

    {

      GYRO_OFFSET_X=tempgx/cnt_g;

      GYRO_OFFSET_Y=tempgy/cnt_g;

      GYRO_OFFSET_Z=tempgz/cnt_g;

      cnt_g = 0;

      GYRO_OFFSET_OK = 1;

    }

    cnt_g++;

  }

  if(!ACC_OFFSET_OK)

  {

    static long int tempax=0,tempay=0,tempaz=0;

    static unsigned char cnt_a=0;

    if(cnt_a==0)

    {

      ACC_OFFSET_X = 0;

      ACC_OFFSET_Y = 0;

      ACC_OFFSET_Z = 0;

      tempax = 0;

      tempay = 0;

      tempaz = 0;

      cnt_a = 1;

    }

    tempax += MPU6050_ACC_LAST_X;//累加

    tempay += MPU6050_ACC_LAST_Y;

    tempaz += MPU6050_ACC_LAST_Z;

    if(cnt_a==200)

    {

      ACC_OFFSET_X = tempax/cnt_a;

      ACC_OFFSET_Y = tempay/cnt_a;

      ACC_OFFSET_Z = tempaz/cnt_a;

      cnt_a = 0;

      ACC_OFFSET_OK = 1;

    }

    cnt_a++;

  }

  //--------------------------------------------//

#else

struct MPU6050Struct *MPU6050WORK;

  MPU6050WORK = ReadMPU6050All();

  MPU6050_ACC_LAST_X = (MPU6050WORK ->MPU6050_ACC_X) - ACC_OFFSET_X;

  MPU6050_ACC_LAST_Y = (MPU6050WORK ->MPU6050_ACC_Y) - ACC_OFFSET_Y;

  MPU6050_ACC_LAST_Z = (MPU6050WORK ->MPU6050_ACC_Z) - ACC_OFFSET_Z;

  MPU6050_GYRO_LAST_X = (MPU6050WORK ->MPU6050_GYRO_X) - GYRO_OFFSET_X;

  MPU6050_GYRO_LAST_Y = (MPU6050WORK ->MPU6050_GYRO_Y) - GYRO_OFFSET_Y;

  MPU6050_GYRO_LAST_Z = (MPU6050WORK ->MPU6050_GYRO_Z) - GYRO_OFFSET_Z;

  if(!GYRO_OFFSET_OK)

  {

    static long int tempgx=0,tempgy=0,tempgz=0;

    static unsigned char cnt_g=0;

    if(cnt_g==0)

    {

      GYRO_OFFSET_X=0;

      GYRO_OFFSET_Y=0;

      GYRO_OFFSET_Z=0;

      tempgx = 0;

      tempgy = 0;

      tempgz = 0;

      cnt_g = 1;

    }

    tempgx+= MPU6050_GYRO_LAST_X;

    tempgy+= MPU6050_GYRO_LAST_Y;

    tempgz+= MPU6050_GYRO_LAST_Z;

    if(cnt_g==200)

    {

      GYRO_OFFSET_X=tempgx/cnt_g;

      GYRO_OFFSET_Y=tempgy/cnt_g;

      GYRO_OFFSET_Z=tempgz/cnt_g;

      cnt_g = 0;

      GYRO_OFFSET_OK = 1;

    }

    cnt_g++;

  }

  if(!ACC_OFFSET_OK)

  {

    static long int tempax=0,tempay=0,tempaz=0;

    static unsigned char cnt_a=0;

    if(cnt_a==0)

    {

      ACC_OFFSET_X = 0;

      ACC_OFFSET_Y = 0;

      ACC_OFFSET_Z = 0;

      tempax = 0;

      tempay = 0;

      tempaz = 0;

      cnt_a = 1;

    }

    tempax += MPU6050_ACC_LAST_X;//累加

    tempay += MPU6050_ACC_LAST_Y;

    tempaz += MPU6050_ACC_LAST_Z;

    if(cnt_a==200)