树莓派驱动6轴传感器MPU6500与SPI初体验(二)

接前回<< 树莓派驱动6轴传感器MPU6500与SPI初体验

          对于一个想做嵌入开发的外围开发者,做硬件相关的应用开发真的是老太太啃骨头。一周以后凭借一点小聪明和给力的度娘,总算在树梅派上实现了SPI接口的基于官放DMP库的姿态融合.之前没有了解DMP(data motion processor)时,一直在网上找相关的姿态结算实现，基本都是滤波+IIC,对于数学白痴的我来说，那些纷乱的代码无法辨别和学习更别提应用了.网上下载代码(IIC的)基本没几个能正常跑的,当然大牛们的代码也不是随便就能让别人跑起来的。总算度娘给立,看到了一篇靠谱的文章<<树莓派 2 通过 I2C 访问 MPU6050 >>感谢作者对github上代码的修正. DMP在6050及6500,9250上都可应用,目前官方最新的库是6.x. 想下载的朋友可fllow上一个连接.这里简单介绍我基于该IIC代码的SPI修改.

       作者提供的代码还是比较清晰的,而且IIC的接口实现都放在libs下.考虑对C不熟,于是就保留了IIC的方法定义,然后在方法体内做SPI的修改,这样就不需要对代码大动干戈了(虽然有点cuo)。

1. SPIdev.h

#ifndef _SPIDEV_H_
#define _SPIDEV_H_

#define SPI_OK 0

#define SPI_ERR -1

int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);

int writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);

int init();

#endif

2.   SPIdev.c

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "SPIdev.h"

int init(){
	// If you call this, it will not actually access the GPIO
	// Use for testing
    //bcm2835_set_debug(1);
    if (!bcm2835_init())
    {
      printf("bcm2835_init failed. Are you running as root??\n");
      return -1;
    }
    if (!bcm2835_spi_begin())
    {
      printf("bcm2835_spi_begin failedg. Are you running as root??\n");
      return -1;
    }
    bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST);      // The default
    bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_256);
	bcm2835_spi_setDataMode(BCM2835_SPI_MODE0);                   // The default
    bcm2835_spi_chipSelect(BCM2835_SPI_CS0);                   // The default
	bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW);
	return 0;
}

int writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {

	char byte[length+1];

	byte[0]=regAddr;

	memcpy(byte+1, data, length);

	bcm2835_spi_transfern(byte,sizeof(byte));

	return 0;
}

int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){

	char byte[length+1];

	byte[0]=regAddr|0x80;

	memcpy(byte+1, data, length);

	bcm2835_spi_transfern(byte,sizeof(byte));

	memcpy(data,byte+1,length);

	return 0;
}

           原程序是直接输出欧拉角,四元数的话也可以直接定义extern打印出来. 官方愿码中还包含指南针和压力传感器的配置,都被我屏蔽掉了。

          最后编译运行打印到串口:

          下一步就是做姿态的动态显示了，先放个图:

 

          本例代码下载:   http://download.csdn.net/detail/ikevin/9690394