四轴飞行器// Copyright (c) 2013
js200300953@qq.com 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/attitude/attitude.c
// 2012-11-7 18:12:59
// js200300953
//attitude 为态度的意思 但是 也有姿态的含义 此为 姿态控制 函数库哦！
#include "bsp/time.h"
#include "app/math/interface.h"
#include "attitude.h"
#include "accfilter.h"
#include "gyrfilter.h"
#include "magfilter.h"
#include "mix.h"
static quaternion attitude; //波：此为 返回四元数的姿态 静态值 调用不释放
//波：但是 此四元数 只能作用在此文件夹下吧 不是全局变量
void attitude_init(void);
const quaternion * attitude_getAttitude(void);
void attitude_inputAcc(const float acc[3]);
void attitude_inputGyr(const float gyr[3]);
void attitude_inputMag(const float mag[3]);
void attitude_mixAccGyrMag(void);
//波：此为 姿态函数 初始化
void attitude_init(void)
{
accfilter_init();
gyrfilter_init();
magfilter_init();
//
quaternion_loadIdentity(&attitude); //波：初始化四元数 1 0 0 0 将attitude初始化了 attitude 姿态
}
// 获取姿态，只读。
//波：这个函数真心没看懂 C语言没过关啊 此函数有返回值 返回的为一个quaternion(四元数的意思)类型的结构体 有
//波：有四部分组成 函数名前的那个指针符号 是什么意思呢 ？ 使用的时候看看怎么用的吧
const quaternion * attitude_getAttitude(void)
{
return &attitude; //波：返回的是一个地址 姿态四元数
}
// acc : 加速度测量值，m/s2。
//波：此为加速度 测量值 输入到此函数 然后调用 加速度滤波处理函数 处理
void attitude_inputAcc(const float acc[3])
{
accfilter_input(acc);
}
// gyr : 角速度测量值，rad/s。
//此为陀螺仪读取值 输入
void attitude_inputGyr(const float gyr[3])
{
gyrfilter_input(gyr);
}
// mag : 磁场方向测量值，标定到长度为1。
void attitude_inputMag(const float mag[3])
{
magfilter_input(mag);
}
//波：此为核心函数 将陀螺仪 加速度 磁场 三者 融合为四元数姿态吧
//波：interval 意思是间隔 也就是此刻的间隔时间
//波：公式 1s=1000 *1000 us
//波：此时调用一个欧拉角转换为四元数的函数 最下面 那句 此为核心哦 ！转过去 读懂吧 ！哈哈
//波：此函数 包括陀螺仪 加速度 欧拉角3变量转换为 四元数 四变量
//波：函数输入包括 姿态,陀螺仪当前值，加速度当前值，距离上次融合时间间隔，
void attitude_mixGyrAccMag(void)
{
//
// 计算积分间隔，判断是否合理。
static uint64_t time_pre_us = 0; //波：定义为静态数据 函数结束后 不释放 起到 记录时间的作用
uint64_t time_now_us = time_nowUs(); //波：当前时间 此为 读取当前时间
int32_t time_interval_us = time_now_us - time_pre_us; //波：计算时间间隔 也就是说 第一次读取时 不准 有开机时间
time_pre_us = time_now_us; //波：将当前时间 设定为过去时间 为了下一个 运算
if(time_interval_us > 1000*1000) // 超过1秒就判为异常，丢弃。 为什么是一秒啊 是不是 太慢了
return; //时间超过一秒 则丢弃 返回 重新开始吧
float time_interval_s = time_interval_us * (1.0f/1e6f); //数据运算 将us转化为s 同时定义了新的变量 time_interval_s
//
//mix_gyrAccMag_crossMethod(&attitude,gyrfilter_current(),accfilter_getCurrent(),magfilter_getCurrent(),time_interval_s);
mix_gyrAcc_crossMethod(&attitude,gyrfilter_current(),accfilter_getCurrent(),time_interval_s);//波：此为欧拉角转换为四元数的函数
//波：之前所有的铺垫 都是为了 这个函数 之前的目的就是得到 时间间隔 也就是 采样时间
//波：加计跟陀螺仪融合函数 因为我们的板子上只有mpu没有电子磁盘
//波：这里的融合 我可以理解为 通过acc修正gyr的pitch 和roll吗 应该是这样的吧 通过测量值也就是观测值acc三个方向
//波：通过转换矩阵 也就是acc在pitch和roll上的分量 来跟gyr得到的值比较 此时不应该是简单的相减 因为是向量
//波：用外积比较好 得到的就是旋转的角度 也就是误差度数 还有长度x*y*sin(i)，i为角度 看看函数吧
//波：然后 再用得到的数值 乘以一定的权值 修正gyr的 roll 和pitch 至于yaw 需要电子磁盘来修正 哦！
//波：四元数 更新姿态 结束 看懂了 但是 本文中没用PI修正误差 而是 直接用了 互补滤波
//波：其中的时间周期 interval 为上次更新到这次更新 间隔的时间
}
