EEPW论坛

前面介绍了crazyflie的数据融合的处理方法，这里对mpu6050的数据按照这种方法处理，得到以下的运行结果。

刚启动时的结果：

稳态建立后的结果为：

上面的数据为加速度的数据，单位为G。可以看到处理后的数据精度比较高，在千分之2左右，效果还是比较好的。

Mahony的算法的解读

    ------本文参考于http://www.eepw.com.cn/article/247809.htm

    本帖目的是通过分析函数，学习四元数的算法，就是新闻里说的“消化吸收再创新”，主要是前两个，至于再创新那就不一定有这个本事了。

    本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。

经常在算法中碰到IMU和AHRS这两个词，关于这两个词的具体含义与区别，这个地址有讨论：http://www.douban.com/note/75915865/，不懂得人可以稍微了解一下。不说别的，直奔主题。

下面开始进入正题：


　　先定义Kp，Ki，以及halfT 。
   　Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度，halfT ，姿态解算时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001

　　#define Kp 2.0f
　　#define Ki 0.002f
　　#define halfT 0.001f

　　初始化四元数
　　float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

　　定义姿态解算误差的积分
　　float exInt = 0, eyInt = 0, ezInt = 0;

　　以下为姿态解算函数。
　　参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
　　参数ax，ay，az分别对应三个轴的加速度原始数据
　　由于加速度的噪声较大，此处应采用滤波后的数据

　　void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
　　{
　　float norm;
　　float vx, vy, vz;
　　float ex, ey, ez;

　　将加速度的原始数据，归一化，得到单位加速度
　　norm = sqrt(ax*ax + ay*ay + az*az);
　　ax = ax / norm;
　　ay = ay / norm;
　　az = az / norm;

　　把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)

　　vx = 2*(q1*q3 - q0*q2);
　　vy = 2*(q0*q1 + q2*q3);
　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

　　这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。

　　在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。

　　向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

　　叉乘是数学基础，维基百科里有详细解释。
　　ex = (ay*vz - az*vy);
　　ey = (az*vx - ax*vz);
　　ez = (ax*vy - ay*vx);

　　将叉乘误差进行积分
　　exInt = exInt + ex*Ki;
　　eyInt = eyInt + ey*Ki;
　　ezInt = ezInt + ez*Ki;

　　用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
　　gx = gx + Kp*ex + exInt;
　　gy = gy + Kp*ey + eyInt;
　　gz = gz + Kp*ez + ezInt;

　　四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨

　　实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。

   　q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
　　q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
　　q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
　　q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

　　四元数单位化
　　norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
　　q0 = q0 / norm;
　　q1 = q1 / norm;
　　q2 = q2 / norm;
　　q3 = q3 / norm;
　　}

　　姿态解算后，就得到了表示姿态的四元数。

开始整通讯了，以前没碰过SPI，找视频学习一下，列出视频地址，共大家同学习。

http://www.iqiyi.com/w_19rrd96ntp.html

看完SPI的视频，一头雾水，还是看看怎么用NFR吧，上网址：

http://wenku.baidu.com/view/35cb65fe0242a8956bece4eb

阅读匿名的NRF驱动程序和数据发送程序，总结一下：

一、端口配置(Spi.h)

void Spi1_Init(void);

//配置SPI的用到的GPIO端口和SPI参数
u8 Spi_RW(u8 dat);

二、驱动程序部分(Nrf2401.h)：

void Nrf24l01_Init(u8 model, u8 ch);

 //初始化,model=1/2/3/4,ch为实用的通道号uint8_t NRF_Read_Reg(uint8_t reg);

//读寄存器
uint8_t NRF_Write_Reg(uint8_t reg, uint8_t value);

//写寄存器
uint8_t NRF_Read_Buf(uint8_t reg, uint8_t *pBuf, uint8_t uchars);

//读缓冲区

void NRF_TxPacket(uint8_t * tx_buf, uint8_t len);

 //发送数据包,用于model 2/4
void NRF_TxPacket_AP(uint8_t * tx_buf, uint8_t len);

//发送数据包,用于model 3

uint8_t Nrf24l01_Check(void);

//自检

三、数据的发送接收程序(Rc.h)：

void Nrf_Check_Event(void);

//判断解锁
void NRF_Send_AF(void);

//发送加速度，角速度，四元数得到的角度等
void NRF_Send_AE(void);

//接收到的RC数据
void NRF_Send_OFFSET(void);

//发送加速度偏移，角速度偏移
void NRF_Send_PID(void);

//发送RPY三个值的PID值
void NRF_Send_ARMED(void);

//发送解锁信息

分析Crazyflie的Nrf24L01的驱动程序：

该代码使用了外部中断，关于外部中断的教程，可见

http://www.cnblogs.com/alvis-jing/p/3678285.html

该博主的其他关于STM32 的文章，见

http://www.cnblogs.com/alvis-jing/category/569663.html，先mark一下。

一、驱动程序(Nrf24l01.h)

/***********************
 * SPI private methods，与匿名的Spi_RW()相同 *
 ***********************/
static char spiSendByte(char byte)
{
  /* Loop while DR register in not emplty */
  while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_TXE) == RESET);

  /* Send byte through the SPI1 peripheral */
  SPI_I2S_SendData(RADIO_SPI, byte);

  /* Wait to receive a byte */
  while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_RXNE) == RESET);

  /* Return the byte read from the SPI bus */
  return SPI_I2S_ReceiveData(RADIO_SPI);
}

static char spiReceiveByte()
{
  return spiSendByte(DUMMY_BYTE);
}

//该函数增加了SPI的中断引脚的定义
void nrfInit(void);

/* Initialisation */
void nrfInit(void)
{
  SPI_InitTypeDef  SPI_InitStructure;
  EXTI_InitTypeDef EXTI_InitStructure;
  GPIO_InitTypeDef GPIO_InitStructure;

  if (isInit)
    return;

  /* Enable the EXTI interrupt router */
  extiInit();

  /* Enable SPI and GPIO clocks */
  RCC_APB2PeriphClockCmd(RADIO_GPIO_SPI_CLK | RADIO_GPIO_CS_PERIF | 
                         RADIO_GPIO_CE_PERIF | RADIO_GPIO_IRQ_PERIF, ENABLE);

  /* Enable SPI and GPIO clocks */
  RCC_APB1PeriphClockCmd(RADIO_SPI_CLK, ENABLE);

  /* Configure main clock */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CLK;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(RADIO_GPIO_CLK_PORT, &GPIO_InitStructure);

  /* Configure SPI pins: SCK, MISO and MOSI */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_SCK |  RADIO_GPIO_SPI_MOSI;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);

  //* Configure MISO */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_MISO;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);

  /* Configure I/O for the Chip select */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CS;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_Init(RADIO_GPIO_CS_PORT, &GPIO_InitStructure);

  /* Configure the interruption (EXTI Source) */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_IRQ;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(RADIO_GPIO_IRQ_PORT, &GPIO_InitStructure);

  GPIO_EXTILineConfig(RADIO_GPIO_IRQ_SRC_PORT, RADIO_GPIO_IRQ_SRC);
  EXTI_InitStructure.EXTI_Line = RADIO_GPIO_IRQ_LINE;
  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  EXTI_Init(&EXTI_InitStructure);

  // Clock the radio with 16MHz
  RCC_MCOConfig(RCC_MCO_HSE);

  /* disable the chip select */
  RADIO_DIS_CS();

  /* Configure I/O for the Chip Enable */
  GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CE;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_Init(RADIO_GPIO_CE_PORT, &GPIO_InitStructure);

  /* disable the chip enable */
  RADIO_DIS_CE();

  /* SPI configuration */
  SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
  SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
  SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
  SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
  SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
  SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
  SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
  SPI_InitStructure.SPI_CRCPolynomial = 7;
  SPI_Init(RADIO_SPI, &SPI_InitStructure);

  /* Enable the SPI  */
  SPI_Cmd(RADIO_SPI, ENABLE);
  
  isInit = true;
}

//测试设备是否已经初始化

bool nrfTest(void);

// Interrupt routine
void nrfIsr(void);

/*** Defines ***/
#define RADIO_RATE_250K 0
#define RADIO_RATE_1M 1
#define RADIO_RATE_2M 2

/* Low level reg access
 * FIXME: the user should not need to access raw registers...
 */

/* Read len bytes from a nRF24L register. 5 Bytes max */
unsigned char nrfReadReg(unsigned char address, char *buffer, int len);
/* Write len bytes a nRF24L register. 5 Bytes max */

unsigned char nrfWriteReg(unsigned char address, char *buffer, int len);

unsigned char nrfRead1Reg(unsigned char address);
/* Write only one byte (useful for most of the reg.) */

unsigned char nrfWrite1Reg(unsigned char address, char byte);

//Interrupt access设置中断回调函数

//回调函数有两个，分别位于Radiolink.c和Eskylink.c
void nrfSetInterruptCallback(void (*cb)(void));

/* Low level functionality of the nrf chip */

/* Sent the NOP command. Used to get the status byte */
unsigned char nrfNop(void);
unsigned char nrfFlushRx(void);
unsigned char nrfFlushTx(void);
// Return the payload length

unsigned char nrfRxLength(unsigned int pipe);
unsigned char nrfActivate(void);
// Write the ack payload of the pipe 0

unsigned char nrfWriteAck(unsigned int pipe, char *buffer, int len);
// Read the RX payload

unsigned char nrfReadRX(char *buffer, int len);

void nrfSetChannel(unsigned int channel);

void nrfSetDatarate(int datarate);

void nrfSetAddress(unsigned int pipe, char* address);

void nrfSetEnable(bool enable);
unsigned char nrfGetStatus(void);

/* 中断服务程序,调用中断回调函数 */
void nrfIsr()