3x8e做的智能监控陀螺仪程序

ID:288254 · 发表于 2018-3-6 16:18

3x8e做的智能监控陀螺仪程序，陀螺仪用的MP6000A
#include "intell_monitor_api.h"
#include "asf.h"
#include "def.h"
#include "string.h"
#include "compiler.h"
#include "stdio.h"
#include "math.h"
#define PI          3.14159265
#define SDA_IO_OUTPUT          0x00
#define SDA_IO_INPUT             0x01
#define IIC_CLOCK_DELAY       100/*20->10, for test*/
#define IIC_HIGH                      0x01
#define IIC_LOW                      0x00
//定义MPU3050内部地址********************
#define WHO    0x75
#define SMPL 0x19
#define DLPF 0x1a
#define GYRO_CONFIG  0x1b //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1c //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define  INT_CONFIG 0x37
#define  INT_ENABLE 0x38
#define  INT_STATUS 0x3a
#define TMP_H 0x41
#define TMP_L 0x42
#define GX_H 0x43
#define GX_L 0x44
#define GY_H 0x45
#define GY_L 0x46
#define GZ_H 0x47
#define GZ_L 0x48
#define AUX_H 0x3b
#define AUX_L 0x3c
#define AUY_H 0x3d
#define AUY_L 0x3e
#define AUZ_H 0x3f
#define AUZ_L 0x40
#define  PWR_M1 0x6b
#define  PWR_M2 0x6c
//****************************
#define SlaveAddress 0xd0
UNIVOLTAGE_t vtg;

static void CLK_Delay(void)
{
unsigned long i;
for (i=0; i<IIC_CLOCK_DELAY; i++);
{
}
}
static STATUS setSCL_GYRO(uchar_t value)
{

  if(value == IIC_LOW)
  {
gpio_set_pin_low(TWI1_CLK_GPIO);
  }
  else
  {
gpio_set_pin_high(TWI1_CLK_GPIO);
  }

return ok;
}
static STATUS SDA_IO_Direction_GYRO(uchar_t value)
{
  if(value == SDA_IO_OUTPUT)  /*定义SDA为输出方向*/
  {
gpio_configure_pin(TWI1_DATA_GPIO,MDIO_OUTPUT_FLAGS);
  }
  if(value == SDA_IO_INPUT) /*定义SDA为输入方向*/
  {
gpio_configure_pin(TWI1_DATA_GPIO,MDIO_INPUT_FLAGS);
  }
CLK_Delay();//delay_us(5);

return ok;
}
static STATUS setSDA_GYRO(uchar_t value)
{
/*SDA_IO_Direction_SFP(SDA_IO_OUTPUT);*/

  if(value == IIC_LOW)
  {
gpio_set_pin_low(TWI1_DATA_GPIO);
  }
  else
  {
gpio_set_pin_high(TWI1_DATA_GPIO);
  }

return ok;
}
static uchar_t  getSDA_GYRO()
{
  if((pio_get_pin_value(TWI1_DATA_GPIO)) == 1)
  {
return IIC_HIGH;
  }
  else
  {
return IIC_LOW;
  }

}

/*读/写操作的START条件*/
static void START_GYRO()
{
SDA_IO_Direction_GYRO(SDA_IO_OUTPUT);
setSDA_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSDA_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
}
/*读/写操作的STOP条件*/
static void STOP_GYRO()
{
/*setSCL_SFP(IIC_LOW);CLK_Delay();*/
SDA_IO_Direction_GYRO(SDA_IO_OUTPUT);

setSDA_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSDA_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
}
/*发送1字节*/
static void BYTE_SND_GYRO(uchar_t WR_data)
{
uchar_t i;
printf("WR_DATA=%x\r\n",WR_data);
/*setSCL_SFP(IIC_LOW);CLK_Delay();*/
SDA_IO_Direction_GYRO(SDA_IO_OUTPUT);
for(i=0;i<8;i++)
{
  setSCL_GYRO(IIC_LOW);
      CLK_Delay();//delay_us(5);
  if(WR_data&0x80)
  {
setSDA_GYRO(IIC_HIGH);
            CLK_Delay();//delay_us(5);
  }
  else
  {
setSDA_GYRO(IIC_LOW);
            CLK_Delay();//delay_us(5);
  }
  WR_data<<=1;
  setSCL_GYRO(IIC_HIGH);
         CLK_Delay();//delay_us(5);
}
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
}
/*接收1字节*/
static uchar_t BYTE_RCV_GYRO()
{
uchar_t temp=0;
uchar_t i;
SDA_IO_Direction_GYRO(SDA_IO_INPUT);
for(i=0;i<8;i++)
{
  temp <<= 1;
  setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
  setSDA_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
  setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
  if(getSDA_GYRO() == IIC_HIGH)
  {
temp |= 0x01;
  }
}
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
return temp;
}
/*发送确认比特（ACK）*/
static void SND_ACK_GYRO()
{
/*setSCL_SFP(IIC_LOW);CLK_Delay();*/
SDA_IO_Direction_GYRO(SDA_IO_OUTPUT);
setSDA_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
}

/*发送非确认比特（noACK）*/
static void SND_noACK_GYRO()
{
/*setSCL_SFP(IIC_LOW);CLK_Delay();*/
SDA_IO_Direction_GYRO(SDA_IO_OUTPUT);
setSDA_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
}
/*接收确认比特，返回ACK/noACK*/
static uchar_t RCV_ACK_GYRO()
{
SDA_IO_Direction_GYRO(SDA_IO_INPUT);
setSCL_GYRO(IIC_HIGH);
CLK_Delay();//delay_us(5);
if(getSDA_GYRO() == IIC_LOW)
{
  setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
  return IIC_LOW;
}
setSCL_GYRO(IIC_LOW);
CLK_Delay();//delay_us(5);
return IIC_HIGH;
}
static STATUS IIC_ERROR_Release_GYRO()
{
   STOP_GYRO();
  printf("error\r\n");
   return error;
}
static STATUS IIC_OK_Release_GYRO()
{
   STOP_GYRO();
   return ok;
}
STATUS sndsIICRead_GYRO(uchar_t slaveAddr, uchar_t readAddr, uchar_t *pBuff)
{
uchar_t regAddr;
unsigned short num;
if (pBuff == NULL)
{
  return  error;
}

   regAddr  = readAddr;
START_GYRO();
BYTE_SND_GYRO(slaveAddr);
if(RCV_ACK_GYRO()==IIC_HIGH)
{
  IIC_ERROR_Release_GYRO();
  return error;
}
BYTE_SND_GYRO(regAddr);
if(RCV_ACK_GYRO()==IIC_HIGH)
{
  IIC_ERROR_Release_GYRO();
  return error;
}
START_GYRO();
BYTE_SND_GYRO(slaveAddr |0x01);
if(RCV_ACK_GYRO()==IIC_HIGH)
{
  IIC_ERROR_Release_GYRO();
  return error;
}

   *pBuff = BYTE_RCV_GYRO();
   SND_ACK_GYRO();
*pBuff = BYTE_RCV_GYRO();
SND_noACK_GYRO();
IIC_OK_Release_GYRO();
return ok;
}

STATUS sndsIICWriteGYRO(uchar_t slaveAddr, uchar_t writeAddr, uchar_t *pBuff)
{
uchar_t regAddr;
unsigned short i;
if (pBuff == NULL)
{
  return  error;
}

   regAddr  = writeAddr;
START_GYRO();
BYTE_SND_GYRO(slaveAddr);
if(RCV_ACK_GYRO()==IIC_HIGH)
{
  IIC_ERROR_Release_GYRO();
  return error;
}
BYTE_SND_GYRO(regAddr);
if(RCV_ACK_GYRO()==IIC_HIGH)
{
  IIC_ERROR_Release_GYRO();
  return error;
}

   BYTE_SND_GYRO(*pBuff);
   SND_ACK_GYRO();
IIC_OK_Release_GYRO();
return ok;
}
void InitMPU6050(void)
{

   delay_ms(500);
uchar_t gyro_value2;
gyro_value2=0x00;
write_GYRO_register(SlaveAddress,PWR_M1,&gyro_value2,1);
delay_ms(10);
gyro_value2=0x07;
write_GYRO_register(SlaveAddress,SMPL,&gyro_value2,1);
delay_ms(10);
gyro_value2=0x06;
write_GYRO_register(SlaveAddress,DLPF,&gyro_value2,1);
delay_ms(10);
gyro_value2=0x18;
write_GYRO_register(SlaveAddress,GYRO_CONFIG,&gyro_value2,1);
delay_ms(10);
gyro_value2=0x01;
write_GYRO_register(SlaveAddress,ACCEL_CONFIG,&gyro_value2,1);
delay_ms(10);

/*uchar_t add,gyro_value1;
for(add=25;add<118;add++)
{
   read_GYRO_register(SlaveAddress,add,&gyro_value1,1);
   printf("REG_VALUE=%x %x\r\n",add,gyro_value1);
delay_ms(10);
}*/
//  write_GYRO_register(SlaveAddress,PWR_M1,0x80,1);
}
#if 0

void InitMPU6050(void)
{
delay_ms(1000);
write_GYRO_register(SlaveAddress,PWR_M1,0x00,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,PWR_M1,0x80,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,SMPL,0x07,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,DLPF,0x06,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,GYRO_CONFIG,0x18,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,ACCEL_CONFIG,0x01,1);
delay_ms(10);
write_GYRO_register(SlaveAddress,INT_CONFIG,0x00,1);
delay_ms(10);
//  write_GYRO_register(SlaveAddress,PWR_M1,0x80,1);
}
#endif
//**************************************
//合成数据
//**************************************
/*ushort_t GetData(uchar_t REG_Address)
{
uchar_t H,L;
H=read_GYRO_register(REG_Address);
L=read_GYRO_register(REG_Address+1);
return (H<<8)+L; //合成数据
}*/
/*float SqrtByNewton(float x)
{
float val = x;//最终
float last;//保存上一个计算的值
do
{
      last = val;
      val =(val + x/val) / 2;
}while(Abs(val-last) > eps);
return val;
}*/
float Sqrt(float x)
{
float xhalf = 0.5f * x;
int i = *(int*)&x;
i = 0x5f375a86 - (i>>1);
x = *(float*)&i;
x = x*(1.5f-xhalf*x*x);
x = x*(1.5f-xhalf*x*x);
x = x*(1.5f-xhalf*x*x);
printf("GYRO_XOUT=%.2f \r\n",1/x);
return 1/x;
}
double atan(double x)
{
//atan(x)=x-x^3/3+x^5/5-x^7/7+.....(-1<x<1)
//return:[-pi/2,pi/2]
double mult,sum,xx;
sum=0;
if(x==1){
      return PI/4;
}
if(x==-1){
      return -PI/4;
}

if((x>1||x<-1))
{
      mult=1/x;
}
else
{
      mult=x;
}
xx=mult*mult;

for(int i=1;i<200;i+=2){
      sum+=mult*((i+1)%4==0?-1:1)/i;
      mult*=xx;
}
if(x>1||x<-1){
      return PI/2-sum;
}
else{
      return sum;
}
}
  /*union UNIVOLTAGE
  {
   float fVoltage;
   char voltage4Byte[4];
  };
  UNIVOLTAGE vtg;
// char RS232ComBuf[4096];
// strcpy(vtg.voltage4Byte, RS232ComBuf);
  //printf("%f", vtg.fVoltage);
*/

void display_who(void)
{
uchar_t gyro_value;
read_GYRO_register(SlaveAddress,WHO,&gyro_value,1);

printf("WHO=%02x\r\n",gyro_value);
//lcd_printf(dis, dis_data);    //转换数据显示
//DisplayListChar(2,0,dis,4); //启始列，行，显示数组，显示长度
}
//显示x轴
void display_x(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
  float x;
  double y;
read_GYRO_register(SlaveAddress,GX_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,GX_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(sshort_t)((gyro_value4<<8)+gyro_value3); //合成数据
// data=data/16.4;
   printf("GYRO_XOUT=%d \r\n",data);
x=Sqrt(Abs(data));
   y=atan(1);
  vtg.fVoltage=y;
  sprintf("changeGYRO_XOUT=%f\r\n", vtg.fVoltage);
      //printf("changeGYRO_XOUT=%d %d \r\n",y,(y*180)/PI);
      //printf("sshort_t=%d \r\n",sizeof(sshort_t));

//data/=16.4;             //计算对应度/秒
   // printf("changeGYRO_XOUT=%d\r\n",data);
/*printf("\r\n");
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_XOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//***********************************************************************
//显示y轴
void display_y(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
  read_GYRO_register(SlaveAddress,GY_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,GY_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(sshort_t)((gyro_value4<<8)+gyro_value3); //合成数据
//data=data/16.4;
printf("GYRO_YOUT=%d \r\n",data);
// dis_data/=16.4;             //计算对应度/秒
  /*    printf("\r\n");
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_YOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//***********************************************************************
//显示z轴
void display_z(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
  read_GYRO_register(SlaveAddress,GZ_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,GZ_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(sshort_t)((gyro_value4<<8)+gyro_value3); //合成数据
//data=data/16.4;
printf("GYRO_ZOUT=%d \r\n",data);
  /*  printf("\r\n");
//dis_data/=16.4;             //计算对应度/秒
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_ZOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}

//显示x_g轴
void display_x_g(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
  read_GYRO_register(SlaveAddress,AUX_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,AUX_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(gyro_value4<<8)|gyro_value3; //合成数据
//data=data/16384;
printf("GYRO_AUXOUT=%d\r\n",data);
/*    printf("\r\n");
//dis_data/=16.4;             //计算对应度/秒
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_AUXOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//***********************************************************************
//显示y轴
void display_y_g(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
read_GYRO_register(SlaveAddress,AUY_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,AUY_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(gyro_value4<<8)|gyro_value3; //合成数据
//data=data/16384;
printf("GYRO_AUYOUT=%d\r\n",data);
/* printf("\r\n");
// dis_data/=16.4;             //计算对应度/秒
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_AUXOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//***********************************************************************
//显示z轴
void display_z_g(void)
{
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
read_GYRO_register(SlaveAddress,AUZ_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,AUZ_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;

data=(gyro_value4<<8)|gyro_value3; //合成数据
//data=data/16384;
printf("GYRO_AUZOUT=%d\r\n",data);
  /*  printf("\r\n");
//dis_data/=16.4;             //计算对应度/秒
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_AUXOUT=%s%s%s%s%s%s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//显示TEM
void display_tem(void)
{
uchar_t fanwei1=0,fanwei2=0;
   uchar_t gyro_value1=0,gyro_value2=0;
   ushort_t gyro_value3=0,gyro_value4=0;
   sshort_t data=0;
sshort_t Temperature=0;
read_GYRO_register(SlaveAddress,TMP_L,&gyro_value1,1);
delay_ms(1);
read_GYRO_register(SlaveAddress,TMP_H,&gyro_value2,1);
gyro_value3=gyro_value1;
gyro_value4=gyro_value2;
data=(gyro_value4<<8)|gyro_value3; //合成数据
Temperature = 35+ ((sshort_t) (data + 13200)) / 280; // 计算出温度
printf("GYRO_TEMPOUT=%d\r\n",Temperature);
   //write_GYRO_register(SlaveAddress,0x19,0x07,1);
//  delay_ms(10);
   //printf("accset=%x\r\n",fanwei);
   read_GYRO_register(SlaveAddress,0x1b,&fanwei1,1);
   printf("GYRO_CONFIG_1B=%x\r\n",fanwei1);
delay_ms(1);
   read_GYRO_register(SlaveAddress,0x1c,&fanwei2,1);
   printf("ACCEL_CONFIG_1C=%x\r\n",fanwei2);
   printf("\r\n");
/*    printf("\r\n");
//dis_data/=16.4;             //计算对应度/秒
lcd_printf(dis, data);    //转换数据显示
printf("changedataGYRO_TEMPOUT=%s %s %s %s %s %s\r\n",dis[0],dis[1],dis[2],dis[3],dis[4],dis[5]);
*/
}
//****************************************
//整数转字符串
//****************************************
void lcd_printf(uchar_t *s,int temp_data)
{
if(temp_data<0)
{
  temp_data=-temp_data;
  *s='-';
}
else *s=' ';
*++s =temp_data/10000+0x30;
temp_data=temp_data%10000;    //取余运算
*++s =temp_data/1000+0x30;
temp_data=temp_data%1000;    //取余运算
*++s =temp_data/100+0x30;
temp_data=temp_data%100;    //取余运算
*++s =temp_data/10+0x30;
temp_data=temp_data%10;    //取余运算
*++s =temp_data+0x30;
}
void IIC_init(void)
{
twi_options_t optsfp;

pmc_enable_periph_clk(BOARD_ID_TWI1);
optsfp.master_clk = sysclk_get_cpu_hz();
optsfp.speed    = TWI_CLK;

if (twi_master_init(BOARD_BASE_TWI1, &optsfp) != TWI_SUCCESS)
{
  puts("-E-\tTWI0 master initialization failed.\r");
}
}
uint8_t read_GYRO_register(uint8_t slave_addr, uint8_t reg_addr, uint8_t *reg_value, uint32_t reg_len)
{
twi_packet_t packet_sfp_rx;
delay_ms(1);
packet_sfp_rx.chip       = slave_addr >> 1;
packet_sfp_rx.addr[0]    = reg_addr;
packet_sfp_rx.addr_length  = TWI_MEM_ADDR_LENGTH;
packet_sfp_rx.buffer    = reg_value;
packet_sfp_rx.length    = reg_len;
  return twi_master_read(BOARD_BASE_TWI1, &packet_sfp_rx);
}
uint8_t write_GYRO_register(uint8_t slave_addr, uint8_t reg_addr, uint8_t *reg_value, uint32_t reg_len)
{
twi_packet_t packet_sfp_tx;

packet_sfp_tx.chip       = slave_addr >> 1;
packet_sfp_tx.addr[0]    = reg_addr;
packet_sfp_tx.addr_length  = TWI_MEM_ADDR_LENGTH;
packet_sfp_tx.buffer    = reg_value;
packet_sfp_tx.length    = reg_len;

  return twi_master_write(BOARD_BASE_TWI1, &packet_sfp_tx);
}

帐号		自动登录	找回密码
密码			立即注册