圆点博士——代码注释——初版——来自波波——sensor.c

四轴飞行器

//     Copyright (c) 2013 [e-mail] All rights reserved.

//         ==================================================

//         ========圆点博士微型四轴飞行器配套软件声明========

//         ==================================================

//     圆点博士微型四轴飞行器配套软件包括上位机程序、下位机Bootloader

// 、下位机App和遥控程序，及它们的源代码，以下总称“软件”。

//     软件仅提供参考，js200300953不对软件作任何担保，不对因使用该软件

// 而出现的损失负责。

//     软件可以以学习为目的修改和使用，但不允许以商业的目的使用该软件。

//     修改该软件时，必须保留原版权声明。

//

//     更多资料见：

// http://blog.sina.com.cn/js200300953

// http://www.etootle.com/

// http://www.eeboard.com/bbs/forum-98-1.html#separatorline

// 圆点博士微型四轴飞行器QQ群：276721324

// app/sensor/sensor.h

// 2012-11-7 13:43:43

// js200300953

#include "bsp/mpu6050.h"

#include "bsp/hmc5883.h"

#include "app/attitude/interface.h"

#include "sensor.h"

void sensor_init(void);

void sensor_checkEvent(void);

void sensor_getRawAcc(int16_t acc[3]);

void sensor_getRawGyr(int16_t gyr[3]);

void sensor_getRawMag(int16_t mag[3]);

void sensor_getCalibratedAcc(float acc[3]);

void sensor_getCalibratedGyr(float gyr[3]);

void sensor_getCalibratedMag(float mag[3]);

void sensor_init(void)

{

}

void sensor_checkEvent(void)

{

if(mpu6050_dataIsReady())

{

float acc[3],gyr[3];

if(mpu6050_read() != 0)

return;

mpu6050_getCalibratedAcc(acc); //波：得到校准后的 加速度值

mpu6050_getCalibratedGyr(gyr); //波：得到校准后的 euler 值

//

// 简单、暴力的高通。

//波：此为 滤波函数 将得到的euler 值 -TH

const static float TH = 0.01; // 阈值，rad/s。

//波：gyr三轴角速度 高通滤波 简单粗暴

for(int i=0;i<3;i++)

{

if(gyr[i] > -TH && gyr[i] < TH)

gyr[i] = 0;

}

//

attitude_inputAcc(acc);  //波：输入加速度计三个数值

attitude_inputGyr(gyr);  //波：输入姿态角

//

attitude_mixGyrAccMag();

}

//

}

//波： 得到mpu6050的 加速度计数值 此为测量标准值

void sensor_getRawAcc(int16_t acc[3])

{

mpu6050_getRawAcc(acc);

}

//波：得到mpu6050的 euler 值 姿态

void sensor_getRawGyr(int16_t gyr[3])

{

mpu6050_getRawGyr(gyr);

}

//波：得到 磁场数值 都是机体坐标系下的

void sensor_getRawMag(int16_t mag[3])

{

hmc5883_getRawMag(mag);

}

//波：得到校正后的 加速度计数值 三个 三轴嘛

void sensor_getCalibratedAcc(float acc[3])

{

mpu6050_getCalibratedAcc(acc);

}

//波：得到校准后的 euler 值 三个

void sensor_getCalibratedGyr(float gyr[3])

{

mpu6050_getCalibratedGyr(gyr);

}

//波： 得到校准后的 磁场值 三个 三轴

void sensor_getCalibratedMag(float mag[3])

{

hmc5883_getCalibratedMag(mag);

}