EEPW论坛

PWM电机驱动实验

STM32参考手册中查到下表（http://blog.csdn.net/dazhaozi/article/details/6868414也有定义）

因为我们的四轴上四个马达接到了PA0~PA3上，所以需要用TIM2的4个channel，不用重映射。

配置PWM：

PWM的输出模式：详细定义见http://blog.sina.com.cn/s/blog_4a3946360100wh5w.html，我们选择PWM1。

贴电机测试的源码：

 * motors.c - Motor driver
 *
 * This code mainly interfacing the PWM peripheral lib of ST.
 */

#include <stdbool.h>

#include "motors.h"

// ST lib includes
#include "stm32f10x_conf.h"

//FreeRTOS includes
#include "FreeRTOS.h"
#include "task.h"

// HW defines
#define MOTORS_GPIO_TIM_PERIF     RCC_APB1Periph_TIM2
#define MOTORS_GPIO_TIM      TIM2
#define MOTORS_GPIO_TIM_DBG  DBGMCU_TIM2_STOP

#define MOTORS_GPIO_PERIF         RCC_APB2Periph_GPIOA
#define MOTORS_GPIO_PORT          GPIOA
#define MOTORS_GPIO_M1            GPIO_Pin_0 // TIM2_CH1
#define MOTORS_GPIO_M2            GPIO_Pin_1 // TIM2_CH2
#define MOTORS_GPIO_M3            GPIO_Pin_2 // TIM2_CH3
#define MOTORS_GPIO_M4            GPIO_Pin_3 // TIM2_CH4

/* Utils Conversion macro */
#define C_BITS_TO_16(X) ((X)<<(16-MOTORS_PWM_BITS))
#define C_16_TO_BITS(X) ((X)>>(16-MOTORS_PWM_BITS)&((1<<MOTORS_PWM_BITS)-1))

const int MOTORS[] = { MOTOR_M1, MOTOR_M2, MOTOR_M3, MOTOR_M4 };
static bool isInit = false;

/* Public functions */

//Initialization. Will set all motors ratio to 0%
void motorsInit()
{
  //Init structures
  GPIO_InitTypeDef GPIO_InitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_OCInitTypeDef  TIM_OCInitStructure;

  if (isInit)
    return;

  //Enable gpio and the timer
  RCC_APB2PeriphClockCmd(MOTORS_GPIO_PERIF, ENABLE);
  RCC_APB1PeriphClockCmd(MOTORS_GPIO_TIM_PERIF, ENABLE);
	
  // Configure the GPIO for the timer output
  GPIO_InitStructure.GPIO_Pin = (MOTORS_GPIO_M1 |
                                 MOTORS_GPIO_M2 |
                                 MOTORS_GPIO_M3 |
                                 MOTORS_GPIO_M4);
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(MOTORS_GPIO_PORT, &GPIO_InitStructure);


  //Timer configuration
  TIM_TimeBaseStructure.TIM_Period = MOTORS_PWM_PERIOD;
  TIM_TimeBaseStructure.TIM_Prescaler = MOTORS_PWM_PRESCALE;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(MOTORS_GPIO_TIM, &TIM_TimeBaseStructure);

  //PWM channels configuration (All identical!)
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_Pulse = 0;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

  TIM_OC1Init(MOTORS_GPIO_TIM, &TIM_OCInitStructure);
  TIM_OC1PreloadConfig(MOTORS_GPIO_TIM, TIM_OCPreload_Enable);

  TIM_OC2Init(MOTORS_GPIO_TIM, &TIM_OCInitStructure);
  TIM_OC2PreloadConfig(MOTORS_GPIO_TIM, TIM_OCPreload_Enable);

  TIM_OC3Init(MOTORS_GPIO_TIM, &TIM_OCInitStructure);
  TIM_OC3PreloadConfig(MOTORS_GPIO_TIM, TIM_OCPreload_Enable);

  TIM_OC4Init(MOTORS_GPIO_TIM, &TIM_OCInitStructure);
  TIM_OC4PreloadConfig(MOTORS_GPIO_TIM, TIM_OCPreload_Enable);

  //Enable the timer
  TIM_Cmd(MOTORS_GPIO_TIM, ENABLE);
  //Enable the timer PWM outputs
  TIM_CtrlPWMOutputs(MOTORS_GPIO_TIM, ENABLE);
  // Halt timer during debug halt.
  DBGMCU_Config(MOTORS_GPIO_TIM_DBG, ENABLE);
  
  isInit = true;
}

bool motorsTest(void)
{
  int i;

  for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++)
  {
    motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
    vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
    motorsSetRatio(MOTORS[i], 0);
    vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
  }
  return isInit;
}


void motorsSetRatio(int id, uint16_t ratio)
{
  switch(id)
  {
    case MOTOR_M1:
      TIM_SetCompare1(MOTORS_GPIO_TIM, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M2:
      TIM_SetCompare2(MOTORS_GPIO_TIM, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M3:
      TIM_SetCompare3(MOTORS_GPIO_TIM, C_16_TO_BITS(ratio));
      break;
    case MOTOR_M4:
      TIM_SetCompare4(MOTORS_GPIO_TIM, C_16_TO_BITS(ratio));
      break;
  }
}

int motorsGetRatio(int id)
{
  switch(id)
  {
    case MOTOR_M1:
      return C_BITS_TO_16(TIM_GetCapture1(MOTORS_GPIO_TIM));
    case MOTOR_M2:
      return C_BITS_TO_16(TIM_GetCapture2(MOTORS_GPIO_TIM));
    case MOTOR_M3:
      return C_BITS_TO_16(TIM_GetCapture3(MOTORS_GPIO_TIM));
    case MOTOR_M4:
      return C_BITS_TO_16(TIM_GetCapture4(MOTORS_GPIO_TIM));
  }

  return -1;
}

// FreeRTOS Task to test the Motors driver
void motorsTestTask(void* params)
{
  static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)};
  int step=0;

  //Wait 3 seconds before starting the motors
  vTaskDelay(M2T(3000));

  while(1)
  {
    motorsSetRatio(MOTOR_M4, sequence[step%4]);
    motorsSetRatio(MOTOR_M3, sequence[(step+1)%4]);
    motorsSetRatio(MOTOR_M2, sequence[(step+2)%4]);
    motorsSetRatio(MOTOR_M1, sequence[(step+3)%4]);

    if(++step>3) step=0;

    vTaskDelay(M2T(1000));
  }
}

下面是main.c文件的内容：

/* Includes */
#include "stm32f10x.h"

/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "misc.h"

#include "led.h"
#include "motors.h"
#include "uart.h"

/* Private functions */
static void prvClockInit(void);
static void prvSetupHardware( void );



int main(void)
{
  prvClockInit();
  /* GPIOD Periph clock enable */
	prvSetupHardware();
	
	xTaskCreate(vLEDTask,(portCHAR *)"LED", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL );
	xTaskCreate(vUartSendTask,(portCHAR *)"UartSend", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL);
	xTaskCreate(motorsTestTask,(portCHAR *)"MotorTest",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+2,NULL);
	xTaskCreate(adcTask,(portCHAR *)"LED",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+3,NULL);
	
	vTaskStartScheduler();

  return 0;	
}

static void prvSetupHardware( void )
{
    SystemInit(); 
    ledInit();
    motorsInit();
    uartInit();
}
/*-----------------------------------------------------------*/

//Clock configuration
static void prvClockInit(void)
{
  ErrorStatus HSEStartUpStatus;

  RCC_DeInit();
  // Enable HSE 
  RCC_HSEConfig(RCC_HSE_ON);
  // Wait till HSE is ready 
  HSEStartUpStatus = RCC_WaitForHSEStartUp();

  if (HSEStartUpStatus == SUCCESS)
  {
    // HCLK = SYSCLK 
    RCC_HCLKConfig(RCC_SYSCLK_Div1);

    // PCLK2 = HCLK 
    RCC_PCLK2Config(RCC_HCLK_Div1);

    // PCLK1 = HCLK/2 
    RCC_PCLK1Config(RCC_HCLK_Div2);

    // Flash 2 wait state 
    FLASH_SetLatency(FLASH_Latency_2);

    // Enable Prefetch Buffer 
    FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);

    // ADCCLK = PCLK2/6 = 72 / 6 = 12 MHz
    RCC_ADCCLKConfig(RCC_PCLK2_Div6);

    // PLLCLK = 16MHz/2 * 9 = 72 MHz 
    RCC_PLLConfig(RCC_PLLSource_HSE_Div2, RCC_PLLMul_9);

    // Enable PLL 
    RCC_PLLCmd(ENABLE);

    // Wait till PLL is ready 
    while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET);

    // Select PLL as system clock source 
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);

    // Wait till PLL is used as system clock source 
    while(RCC_GetSYSCLKSource() != 0x08);
  } else {
      GPIO_InitTypeDef GPIO_InitStructure;
  
    //Cannot start the main oscillator: LED of death...
    GPIO_InitStructure.GPIO_Pin = LED2_GPIO;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;

    GPIO_Init(LED2_GPIO_PORT, &GPIO_InitStructure);
    
    GPIO_InitStructure.GPIO_Pin = LED3_GPIO;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;

    GPIO_Init(LED3_GPIO_PORT, &GPIO_InitStructure);

    GPIO_ResetBits(LED2_GPIO_PORT, LED2);
    GPIO_ResetBits(LED3_GPIO_PORT, LED3);

    //Cannot start xtal oscillator!
    while(1); 
  }
}

运行后电机动起来了

ADC电池电压监测

我们用的板子VBAT接PB1端口，查表：

选择ADC1，通道9。

ADC的工作模式等定义，见http://blog.csdn.net/fouder_li/article/details/6718884

ADC终于打印数据了，前者是后者的2倍左右，是因为vbat经过2分压后传递给PB1接口的。

程序沿用crazyflie的程序，修改端口的定义和外部触发的定时器。

1、GPIO_VBAT 修改为GPB1。

2、参照上表，GPB1 对于的ADC Channel为 ADC_Channel_9。

3、修改ADC_ExternalTrigConv为T4CC4。

crazyflie使用了TIM2的Channel2，因为我们的电机接了TIM2，所以在这里改成了T4C4。

4、修改定时器及通道为TIM4，Channel4。

5、修改adc.h文件里的ADC_INTERNAL_VREF，此处原值为1.20，应该改为3.30。

暂时就这么多吧，下面是adc.c的代码，有错误请指出。

#include "stm32f10x_conf.h"

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "adc.h"
#include "pm.h"
#include "nvicconf.h"
#include "imu.h"

#include "uart.h"
//#include "acc.h"

// PORT A
#define GPIO_VBAT        GPIO_Pin_1

// CHANNELS
#define NBR_OF_ADC_CHANNELS   1
#define CH_VBAT               ADC_Channel_9

#define CH_VREF               ADC_Channel_17
#define CH_TEMP               ADC_Channel_16

static bool isInit;
volatile AdcGroup adcValues[ADC_MEAN_SIZE * 2];

xQueueHandle      adcQueue;

static void adcDmaInit(void)
{
  DMA_InitTypeDef DMA_InitStructure;

  RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);

  // DMA channel1 configuration
  DMA_DeInit(DMA1_Channel1);
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&adcValues;
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
  DMA_InitStructure.DMA_BufferSize = NBR_OF_ADC_CHANNELS * (ADC_MEAN_SIZE * 2);
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
  DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
  DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
  DMA_Init(DMA1_Channel1, &DMA_InitStructure);
  // Enable DMA channel1
  DMA_Cmd(DMA1_Channel1, ENABLE);
}

/**
 * Decimates the adc samples after oversampling
 */
static void adcDecimate(AdcGroup* oversampled, AdcGroup* decimated)
{
  uint32_t i, j;
  uint32_t sum;
  uint32_t sumVref;
  AdcGroup* adcIterator;
  AdcPair *adcOversampledPair;
  AdcPair *adcDecimatedPair;

  // Compute sums and decimate each channel
  adcDecimatedPair = (AdcPair*)decimated;
  for (i = 0; i < NBR_OF_ADC_CHANNELS; i++)
  {
    adcIterator = oversampled;
    sum = 0;
    sumVref = 0;
    for (j = 0; j < ADC_MEAN_SIZE; j++)
    {
      adcOversampledPair = &((AdcPair*)adcIterator)[i];
      sum += adcOversampledPair->val;
      sumVref += adcOversampledPair->vref;
      adcIterator++;
    }
    // Decimate
    adcDecimatedPair->val = sum / ADC_DECIMATE_DIVEDEND;
    adcDecimatedPair->vref = sumVref / ADC_DECIMATE_DIVEDEND;
    adcDecimatedPair++;
  }
}

void adcInit(void)
{

	GPIO_InitTypeDef GPIO_InitStructure;
  ADC_InitTypeDef ADC_InitStructure;
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;

  if(isInit)
    return;

  // Enable TIM4, GPIOA and ADC1 clock
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 |
                         RCC_APB2Periph_GPIOA, ENABLE);

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
  GPIO_Init(GPIOB, &GPIO_InitStructure);				

  //Timer configuration
  TIM_TimeBaseStructure.TIM_Period = ADC_TRIG_PERIOD;
  TIM_TimeBaseStructure.TIM_Prescaler = ADC_TRIG_PRESCALE;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

  // TIM4 channel4 configuration in PWM mode
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_Pulse = 1;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  TIM_OC4Init(TIM4, &TIM_OCInitStructure);
  TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
  // Halt timer 2 during debug halt.
  DBGMCU_Config(DBGMCU_TIM4_STOP, ENABLE);

  adcDmaInit();

  // ADC1 configuration
  ADC_DeInit(ADC1);
  ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
  ADC_InitStructure.ADC_ScanConvMode = ENABLE;
  ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T4_CC4;
  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
  ADC_InitStructure.ADC_NbrOfChannel = NBR_OF_ADC_CHANNELS;
  ADC_Init(ADC1, &ADC_InitStructure);

  // ADC1 channel sequence
  ADC_RegularChannelConfig(ADC1, CH_VREF, 1, ADC_SampleTime_28Cycles5);

  // ADC2 configuration
  ADC_DeInit(ADC2);
  ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
  ADC_InitStructure.ADC_ScanConvMode = ENABLE;
  ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
  ADC_InitStructure.ADC_NbrOfChannel = NBR_OF_ADC_CHANNELS;
  ADC_Init(ADC2, &ADC_InitStructure);

  // ADC2 channel sequence
  ADC_RegularChannelConfig(ADC2, CH_VBAT, 1, ADC_SampleTime_28Cycles5);

  // Enable ADC1
  ADC_Cmd(ADC1, ENABLE);
  // Calibrate ADC1
  ADC_ResetCalibration(ADC1);
  while(ADC_GetResetCalibrationStatus(ADC1));
  ADC_StartCalibration(ADC1);
  while(ADC_GetCalibrationStatus(ADC1));

  // Enable ADC1 external trigger
  ADC_ExternalTrigConvCmd(ADC1, ENABLE);
  ADC_TempSensorVrefintCmd(ENABLE);

  // Enable ADC2
  ADC_Cmd(ADC2, ENABLE);
  // Calibrate ADC2
  ADC_ResetCalibration(ADC2);
  while(ADC_GetResetCalibrationStatus(ADC2));
  ADC_StartCalibration(ADC2);
  while(ADC_GetCalibrationStatus(ADC2));

  // Enable ADC2 external trigger
  ADC_ExternalTrigConvCmd(ADC2, ENABLE);

  // Enable the DMA1 channel1 Interrupt
  NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_ADC_PRI;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  adcQueue = xQueueCreate(1, sizeof(AdcGroup*));
  xTaskCreate(adcTask, (const signed char *)"ADC", configMINIMAL_STACK_SIZE, NULL, /*priority*/3, NULL);

	printf("this is in adcInit\r\n");

  isInit = true;
}

bool adcTest(void)
{
  return isInit;
}

float adcConvertToVoltageFloat(uint16_t v, uint16_t vref)
{
  return (（v<<1） / (vref / ADC_INTERNAL_VREF));
}

void adcDmaStart(void)
{
  // Enable the Transfer Complete and Half Transfer Interrupt
  DMA_ITConfig(DMA1_Channel1, DMA_IT_TC | DMA_IT_HT, ENABLE);
  // Enable ADC1 DMA
  ADC_DMACmd(ADC1, ENABLE);
  // TIM4 counter enable
  TIM_Cmd(TIM4, ENABLE);
}

void adcDmaStop(void)
{
//  TIM_Cmd(TIM4, DISABLE);
}

void adcInterruptHandler(void)
{
  portBASE_TYPE xHigherPriorityTaskWoken;
  AdcGroup* adcBuffer;

  if(DMA_GetITStatus(DMA1_IT_HT1))
  {
    DMA_ClearITPendingBit(DMA1_IT_HT1);
    adcBuffer = (AdcGroup*)&adcValues[0];
    xQueueSendFromISR(adcQueue, &adcBuffer, &xHigherPriorityTaskWoken);
  }
  if(DMA_GetITStatus(DMA1_IT_TC1))
  {
    DMA_ClearITPendingBit(DMA1_IT_TC1);
    adcBuffer = (AdcGroup*)&adcValues[ADC_MEAN_SIZE];
    xQueueSendFromISR(adcQueue, &adcBuffer, &xHigherPriorityTaskWoken);
  }
}

void adcTask(void *param)
{
  AdcGroup* adcRawValues;
  AdcGroup adcValues;

  vTaskSetApplicationTaskTag(0, (pdTASK_HOOK_CODE)TASK_ADC_ID_NBR);
  vTaskDelay(1000);

  adcDmaStart();

  while(1)
  {
    xQueueReceive(adcQueue, &adcRawValues, portMAX_DELAY);
    adcDecimate(adcRawValues, &adcValues);  // 10% CPU
//    pmBatteryUpdate(&adcValues);

    vTaskDelay(250);
    printf(" vbat.val: %04x, vbat.vref: %04x\r\n",adcValues.vbat.val,adcValues.vbat.vref);
//    uartSendData(sizeof(AdcGroup)*ADC_MEAN_SIZE, (uint8_t*)adcRawValues);

  }
}

NRF无线通讯实验

此处要自己写通讯协议？用于遥控器和飞行器交互通讯。

通讯协议已经分析了，我是传送门

前段时间歇了一段时间，完了一段时间的树莓派，现在重拾旧业，弄了几天的nrf24l01+，终于实现了通信，虽花了大把的时间，但也是很有收获。

工程是在crazyflie的工程基础上修改得到的。运行了freeRTOS实时操作系统，在接收到中断后通过发送信号量等过程最后收到数据，因此程序比较复杂。在调试中对函数中断，中断服务程序编写，中断线程，及nrf24l01的设置等都慢慢克服，终于实现了遥控器发送，四轴接收的功能。

录视频太麻烦，来张图片简单见证：

MPU6050传感器驱动实验

用crazyflie的I2C程序，要改的地方比较多，找到其他资料：

http://blog.csdn.net/elong_2009/article/details/8189700

先保存，看看再说。

在crazyflie的I2C程序中，关于DMA的Channel，相关资料见：

http://blog.csdn.net/zzwdkxx/article/details/9026173

STM32单片机的IIC硬件编程---查询等待方式

http://blog.csdn.net/WangSanHuai2010/article/details/5652071

关于MPU6050的详细资料，可以找相关文档，下面的网址上的说明也比较详细，需要了解的可以去看看：

http://blog.sina.com.cn/s/blog_8240cbef01018i10.html

今天调通了MPU6050，工程文件及结果见http://forum.eepw.com.cn/thread/251222/3#27

传感器滤波算法分析

    由于加速度模块的精度差，但没有长时间的漂移，因此我们需要对测得的加速度的数据进行处理。
    让四轴飞网友移植的匿名四轴的处理是对连续采集到的20个数据进行滑动平均滤波，所谓滑动平均滤波就是开辟N个数据暂存区来存放获取的最新N个数据，然后对其进行平均。
下面是滤波程序：

上面的算法比较简单，即为最后20次输入结果的平均值。 下面分析另一种滤波算法，Crazyflie的工程，采用的是iir滤波算法，算法如下：

static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out, Axis3i32*  storedValues, int32_t attenuation)
{
  out->x = iirLPFilterSingle(in->x, attenuation, &storedValues->x);
  out->y = iirLPFilterSingle(in->y, attenuation, &storedValues->y);
  out->z = iirLPFilterSingle(in->z, attenuation, &storedValues->z);
}

对上面的滤波函数分析：

static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out, Axis3i32* storedValues, int32_t attenuation)

函数对三个方向的加速度值分别调用滤波函数：  

 分析iirLPFilterSingle函数：

1、  int32_t filttmp = *filt; 
 新建32bit的filttmp变量，初始化为storedValues，在后面的程序中可以看出每次滤波后同时更新storedValues的值。 

 2、规范衰减系数  IIR_SHIFT = 8

  if (attenuation > (1<<IIR_SHIFT))
  {
    attenuation = (1<<IIR_SHIFT);
  }
  else if (attenuation < 1)
  {
    attenuation = 1;
  }

3、计算filttmp  
  // Shift to keep accuracy
  inScaled = in << IIR_SHIFT;
  // Calculate IIR filter
  filttmp = filttmp + (((inScaled-filttmp) >> IIR_SHIFT) * attenuation);

将上面的两个语句化简，即： 
  filttmp = filttmp + (256*in - filttmp)*(attenuation/256) //注：该化简有稍微误差

           = (256*in)*(attenuation/256) + filttmp*(1-attenuation/256)

 4、*filt = filttmp;
 
即storedValues 
    = filttmp 
    = (256*in)*(attenuation/256) + filttmp*(1-attenuation/256)，

更新storedValues的值

 5、输出结果out

姿态解算

姿态解算，网上用得最多的就是先求四元数，再转化成角度了。资料显示，相对于另两种旋转表示法（矩阵和欧拉角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等 。

下面列出几个常用的算法，都是用C语言的格式。

1、让四轴飞的匿名四轴版

*************************************************

//默认参数  10 0.008 
#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                       // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                    // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float norm;
//  float hx, hy, hz, bx, bz;
  float vx, vy, vz;// wx, wy, wz;
  float ex, ey, ez;
	
  // 先把这些用得到的值算好
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
//  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
//  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;
	
	if(ax*ay*az==0)
 		return;
		
  norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  // estimated direction of gravity and flux (v and w)      估计重力方向和流量/变迁
  vx = 2*(q1q3 - q0q2);												//四元素中xyz的表示
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  // error is sum of cross product between reference direction of fields and direction measured by sensors
  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;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;					   							//将误差PI后补偿到陀螺仪，即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;				   							//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

  // integrate quaternion rate and normalise						   //四元素的微分方程
  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;

  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Q_ANGLE.Z = GYRO_I.Z;//atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

*************************************************

下面两个算法为crazyflie使用的算法

变量定义如下：

//#define MADWICK_QUATERNION_IMU

#ifdef MADWICK_QUATERNION_IMU
  #define BETA_DEF     0.01f    // 2 * proportional gain
#else // MAHONY_QUATERNION_IMU
    #define TWO_KP_DEF  (2.0f * 0.4f) // 2 * proportional gain
    #define TWO_KI_DEF  (2.0f * 0.001f) // 2 * integral gain
#endif

#ifdef MADWICK_QUATERNION_IMU
  float beta = BETA_DEF;     // 2 * proportional gain (Kp)
#else // MAHONY_QUATERNION_IMU
  float twoKp = TWO_KP_DEF;    // 2 * proportional gain (Kp)
  float twoKi = TWO_KI_DEF;    // 2 * integral gain (Ki)
  float integralFBx = 0.0f;
  float integralFBy = 0.0f;
  float integralFBz = 0.0f;  // integral error terms scaled by Ki
#endif

float q0 = 1.0f;
float q1 = 0.0f;
float q2 = 0.0f;
float q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame

*************************************************

2、crazyflie的Madgwick's IMU and AHRS algorithms

*************************************************

// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date     Author          Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt)
{
  float recipNorm;
  float s0, s1, s2, s3;
  float qDot1, qDot2, qDot3, qDot4;
  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

  // Rate of change of quaternion from gyroscope
  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
  {
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Auxiliary variables to avoid repeated arithmetic
    _2q0 = 2.0f * q0;
    _2q1 = 2.0f * q1;
    _2q2 = 2.0f * q2;
    _2q3 = 2.0f * q3;
    _4q0 = 4.0f * q0;
    _4q1 = 4.0f * q1;
    _4q2 = 4.0f * q2;
    _8q1 = 8.0f * q1;
    _8q2 = 8.0f * q2;
    q0q0 = q0 * q0;
    q1q1 = q1 * q1;
    q2q2 = q2 * q2;
    q3q3 = q3 * q3;

    // Gradient decent algorithm corrective step
    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
    s0 *= recipNorm;
    s1 *= recipNorm;
    s2 *= recipNorm;
    s3 *= recipNorm;

    // Apply feedback step
    qDot1 -= beta * s0;
    qDot2 -= beta * s1;
    qDot3 -= beta * s2;
    qDot4 -= beta * s3;
  }

  // Integrate rate of change of quaternion to yield quaternion
  q0 += qDot1 * dt;
  q1 += qDot2 * dt;
  q2 += qDot3 * dt;
  q3 += qDot4 * dt;

  // Normalise quaternion
  recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

*************************************************

3、crazyflie的Mayhony's AHRS algorithm

*************************************************

// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date     Author      Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt)
{
  float recipNorm;
  float halfvx, halfvy, halfvz;
  float halfex, halfey, halfez;
  float qa, qb, qc;

  gx = gx * M_PI / 180;
  gy = gy * M_PI / 180;
  gz = gz * M_PI / 180;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
  {
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity and vector perpendicular to magnetic flux
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;

    // Error is sum of cross product between estimated and measured direction of gravity
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f)
    {
      integralFBx += twoKi * halfex * dt;  // integral error scaled by Ki
      integralFBy += twoKi * halfey * dt;
      integralFBz += twoKi * halfez * dt;
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else
    {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

  // Integrate rate of change of quaternion
  gx *= (0.5f * dt);   // pre-multiply common factors
  gy *= (0.5f * dt);
  gz *= (0.5f * dt);
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

*************************************************

匿名的程序在更新四元数的同时更新了姿态角，crazyflie使用单独的函数获得姿态角，同时使用了一个快速求平方根的逆的函数invSqrt.代码如下：

void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
  float gx, gy, gz; // estimated gravity direction

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

  *yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1) * 180 / M_PI;
  *pitch = atan(gx / sqrt(gy*gy + gz*gz)) * 180 / M_PI;
  *roll = atan(gy / sqrt(gx*gx + gz*gz)) * 180 / M_PI;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
}

*************************************************

本帖就贴些代码了，下一帖再分析代码。

今天总算有点进展了，以前总觉得定时不准，终于查出原因了。在原来的main()函数中(可在《PWM电机驱动》章看到)，在配置了外部系统时钟后，又重新调用了sysInit()函数，该函数仍然是配置时钟，这样，开始配置的时钟不再有效，贴源码：

/* Personal configs */
#include "FreeRTOSConfig.h"

/* FreeRtos includes */
#include "FreeRTOS.h"
#include "task.h"

/* Project includes */
#include "config.h"
#include "system.h"
#include "nvic.h"

#include "usec_time.h"

#include "led.h"
#include "uart.h"
#include "motors.h"
#include "adc.h"
#include "imu.h"

/* ST includes */
#include "stm32f10x.h"

/* Private functions */
static void prvClockInit(void);
static void prvSetupHardware( void );

int main() 
{
  //Low level init: Clock and Interrupt controller
  prvSetupHardware();

  nvicInit();

  uartInit();
  xTaskCreate(vUartSendTask,(const signed char *)"UartSend", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL);

  ledInit();
  xTaskCreate(vLEDTask,(const signed char *)"LED", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL );

  motorsInit();
//  xTaskCreate(motorsTestTask,(const signed  char *)"MotorTest",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+3,NULL);

  adcInit();
	
	
 // imu6Test();
	
  //Launch the system task that will initialize and start everything
//  systemLaunch();
//  xTaskCreate(vImu6ReadTask,(const signed char *)"Imu6Read", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL);
  
  //Start the FreeRTOS scheduler
  vTaskStartScheduler();

  //Should never reach this point!
  while(1);

 // return 0;
}

static void prvSetupHardware( void )
{
  SystemInit(); 
  prvClockInit();
}

//Clock configuration
static void prvClockInit(void)
{
  ErrorStatus HSEStartUpStatus;

  RCC_DeInit();
  /* Enable HSE */
  RCC_HSEConfig(RCC_HSE_ON);
  /* Wait till HSE is ready */
  HSEStartUpStatus = RCC_WaitForHSEStartUp();

	if (HSEStartUpStatus == SUCCESS)
  {
    /* Enable Prefetch Buffer */
    FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);

    /* Flash 2 wait state */
    FLASH_SetLatency(FLASH_Latency_2);

    /* HCLK = SYSCLK */
    RCC_HCLKConfig(RCC_SYSCLK_Div1);

    /* PCLK2 = HCLK */
    RCC_PCLK2Config(RCC_HCLK_Div1);

    /* PCLK1 = HCLK/2 */
    RCC_PCLK1Config(RCC_HCLK_Div2);

    /* ADCCLK = PCLK2/6 = 72 / 6 = 12 MHz*/
    RCC_ADCCLKConfig(RCC_PCLK2_Div6);

    /* PLLCLK = 16MHz/2 * 9 = 72 MHz */
    RCC_PLLConfig(RCC_PLLSource_HSE_Div2, RCC_PLLMul_9);

    /* Enable PLL */
    RCC_PLLCmd(ENABLE);

    /* Wait till PLL is ready */
    while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET);

    /* Select PLL as system clock source */
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);

    /* Wait till PLL is used as system clock source */
    while(RCC_GetSYSCLKSource() != 0x08);
  } else {
      GPIO_InitTypeDef GPIO_InitStructure;
  
    //Cannot start the main oscillator: LED of death...
    GPIO_InitStructure.GPIO_Pin = LED2_GPIO;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;

    GPIO_Init(LED2_GPIO_PORT, &GPIO_InitStructure);
    
    GPIO_InitStructure.GPIO_Pin = LED3_GPIO;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;

    GPIO_Init(LED3_GPIO_PORT, &GPIO_InitStructure);

    GPIO_ResetBits(LED2_GPIO_PORT, LED2);
    GPIO_ResetBits(LED3_GPIO_PORT, LED3);

    //Cannot start xtal oscillator!
    while(1); 
  }
}

运行后调用原始函数vTaskDelay()后延时准确了。

重新修改了ADC的程序模块，沿用了crazyflie的程序，修改情况见《ADC电压监测》