茶哥的一段关于飞控算法的程序我在看时遇到了问题
#include <math.h>#include "suanfa.h"
#include "myiic.h"
#include "timer.h"
#include "delay.h"
#include "output.h"
/*数据类型转换，float型-double型支持相互识别+-；已经验证
然而short型和float/double型不支持识别+-!！所以添加转换函数。 */
void ACCEL_F_Data(float *cord,unsigned int *timerc)
{
#define g 10.0
#define cici 5
#define tine 0.00001
static signed short S_F; //当需要保留函数上一次调用结束时的值时就要用到静态局部变量
static unsigned char i;
////////////////////////////// <ACCELXY:16384.0LSB> <GYROXY:65.5LSB>
static double DATA_Deal; //double型中间变量用于abs,asin,acos,atan等函数
static float X_Data0; //前一刻最优估计X
static float Y_Data0; //前一刻最优估计Y
static float X_Data_cova0; //前一刻X最优估计协方差
static float Y_Data_cova0; //前一刻Y最优估计协方差
////////////////////////////////////////////////////////////////////////ACCEL_XYOUT 剪头方向为负
static float X_Data1; //当前刻最优估计X
static float Y_Data1; //当前刻最优估计Y
static float X_ACCEL_Data;//当前刻测量值X数据
static float Y_ACCEL_Data;//当前刻测量值Y数据
static float X_ACCEL_Data_avr;//当前刻测量值X平均值
static float Y_ACCEL_Data_avr;//当前刻测量值Y平均值
static float X_ACCEL_Data_cova; //当前刻测量值X协方差
static float Y_ACCEL_Data_cova; //当前刻测量值Y协方差
static float X_GYRO_Data; //当前刻X状态控制量
static float Y_GYRO_Data; //当前刻Y状态控制量
static float X_GYRO_Data_avr; //当前刻X状态控制量平均值
static float Y_GYRO_Data_avr; //当前刻Y状态控制量平均值
static float X_GYRO_Data_cova;//当前刻X状态控制量协方差
static float Y_GYRO_Data_cova;//当前刻Y状态控制量协方差
X_Data0=cord; //cord为前一刻最优估计X 单位°
X_Data_cova0=cord; //cord为前一刻最优估计X协方差
Y_Data0=cord; //cord为前一刻最优估计Y 单位°
Y_Data_cova0=cord; //cord为前一刻最优估计Y协方差
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for(i=0;i<cici;i++)
{
S_F=(IIC_Read(ACCEL_XOUT_H)<<8)|(IIC_Read(ACCEL_XOUT_L)); //当前X测量值
X_ACCEL_Data=((float)S_F); //类型转换 c语言分配float型数据4个字节，得到6位有效数字
S_F=(IIC_Read(GYRO_YOUT_H)<<8)|(IIC_Read(GYRO_YOUT_L)); //当前X控制量 Y_GYRO_Data-=50; 朝箭头为正
X_GYRO_Data=((float)S_F); //类型转换
X_GYRO_Data-=50;
S_F=(IIC_Read(ACCEL_YOUT_H)<<8)|(IIC_Read(ACCEL_YOUT_L)); //当前Y测量值
Y_ACCEL_Data=((float)S_F); //类型转换
S_F=(IIC_Read(GYRO_XOUT_H)<<8)|(IIC_Read(GYRO_XOUT_L)); //当前Y控制量 X _GYRO_Data+=140;朝箭头为负= =
Y_GYRO_Data=((float)S_F); //类型转换
Y_GYRO_Data+=140;
}
for(i=0,X_ACCEL_Data_avr=0,Y_ACCEL_Data_avr=0,X_GYRO_Data_avr=0,Y_GYRO_Data_avr=0;i<cici;i++)
{
X_ACCEL_Data_avr+=X_ACCEL_Data;
Y_ACCEL_Data_avr+=Y_ACCEL_Data;
X_GYRO_Data_avr+=X_GYRO_Data;
Y_GYRO_Data_avr+=Y_GYRO_Data;
}
X_ACCEL_Data_avr/=cici; //当前X测量值平均值
Y_ACCEL_Data_avr/=cici; //当前Y测量值平均值
X_GYRO_Data_avr/=cici; //当前X状态控制量平均值
Y_GYRO_Data_avr/=cici; //当前X状态控制量平均值
for(i=0,X_ACCEL_Data_cova=0,X_GYRO_Data_cova=0,Y_ACCEL_Data_cova=0,Y_GYRO_Data_cova=0;i<cici;i++)
{
//平均值即方差的期望值
X_ACCEL_Data_cova+=(X_ACCEL_Data-X_ACCEL_Data_avr)*(X_ACCEL_Data-X_ACCEL_Data_avr);
X_GYRO_Data_cova+=(X_GYRO_Data-X_GYRO_Data_avr)*(X_GYRO_Data-X_GYRO_Data_avr);
Y_ACCEL_Data_cova+=(Y_ACCEL_Data-Y_ACCEL_Data_avr)*(Y_ACCEL_Data-Y_ACCEL_Data_avr);
Y_GYRO_Data_cova+=(Y_GYRO_Data-Y_GYRO_Data_avr)*(Y_GYRO_Data-Y_GYRO_Data_avr);
}
//c语言在处理float型数据时会将float型数据先转化为double型数据进行处理
X_ACCEL_Data_cova/=cici; //当前测量值X协方差 需要转换为°
DATA_Deal=X_ACCEL_Data_cova;
DATA_Deal=(DATA_Deal/16384.0)*10;
X_ACCEL_Data_cova=asin(DATA_Deal/g)*57.3; //测量值X协方差°
//-----------------------------------------
Y_ACCEL_Data_cova/=cici; //当前测量值Y协方差 需要转换为°
DATA_Deal=Y_ACCEL_Data_cova;
DATA_Deal=(DATA_Deal/16384.0)*10;
Y_ACCEL_Data_cova=asin(DATA_Deal/g)*57.3; //测量值Y协方差°
///////////////////////////////////////////
X_GYRO_Data_cova/=cici; //当前X状态控制量协方差 需要转换为°
X_GYRO_Data_cova=(X_Data_cova0+X_GYRO_Data_cova)/2*tine*timerc/65.5; //X控制量协方差°
//-----------------------------------------
Y_GYRO_Data_cova/=cici; //当前Y状态控制量协方差 需要转换为°
Y_GYRO_Data_cova=(Y_Data_cova0+Y_GYRO_Data_cova)/2*tine*timerc/65.5; //Y控制量协方差°
///////////////////////////////////////////////////////////////////////ACCEL_OUTXY单位转换
DATA_Deal=X_ACCEL_Data_avr;
X_ACCEL_Data_avr=1; //为角度的正负做准备
if(DATA_Deal<0){
DATA_Deal=(-1)*DATA_Deal;
X_ACCEL_Data_avr=-1; //角度为负
}
DATA_Deal=(DATA_Deal/16384.0)*10; //g = 10
DATA_Deal=asin(DATA_Deal/g)*57.3; //结果是弧度制 转换为角度
X_ACCEL_Data_avr*=DATA_Deal; //当前X测量值 单位°
DATA_Deal=cos(DATA_Deal);
//-----------------------------------------
DATA_Deal=Y_ACCEL_Data_avr;
Y_ACCEL_Data_avr=1; //为角度的正负做准备
if(DATA_Deal<0){
DATA_Deal=(-1)*DATA_Deal;
Y_ACCEL_Data_avr=-1; //角度为负
}
DATA_Deal=(DATA_Deal/16384.0)*10; //g = 10
DATA_Deal=asin(DATA_Deal/g)*57.3; //结果是弧度制 转换为角度
Y_ACCEL_Data_avr*=DATA_Deal; //当前Y测量值 单位°
////////////////////////////////////////////////////////////////////////GYRO_XY单位转换
X_GYRO_Data_avr/=65.5; //当前控制量x值 单位°/S
//-----------------------------------------
Y_GYRO_Data_avr/=65.5; //当前控制量x值 单位°/S
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
X_Data0=X_Data0-(X_GYRO_Data_avr*timerc*tine); //当前预测值 赋值X_data0
//-----------------------------------------
Y_Data0=Y_Data0+(Y_GYRO_Data_avr*timerc*tine); //当前预测值 赋值Y_data0
timerc=0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
DATA_Deal=X_GYRO_Data_cova/(X_GYRO_Data_cova+X_ACCEL_Data_cova);//协方差X 权重比
if(DATA_Deal>=1.0)DATA_Deal=0;
X_Data1=X_Data0+(X_ACCEL_Data_avr-X_Data0)*DATA_Deal; //得到当前X最优估计值
cord=X_Data1; //记录当前X最优估计值
cord=X_GYRO_Data_cova+(X_ACCEL_Data_cova-X_GYRO_Data_cova)*DATA_Deal; //记录当前X最优估计协方差
//-----------------------------------------
DATA_Deal=Y_GYRO_Data_cova/(Y_GYRO_Data_cova+Y_ACCEL_Data_cova);//协方差Y 权重比
if(DATA_Deal>=1.0)DATA_Deal=0;
Y_Data1=Y_Data0+(Y_ACCEL_Data_avr-Y_Data0)*DATA_Deal; //得到当前Y最优估计值
cord=Y_Data1; //记录当前Y最优估计值
cord=Y_GYRO_Data_cova+(Y_ACCEL_Data_cova-Y_GYRO_Data_cova)*DATA_Deal; //记录当前Y最优估计协方差
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
cord=X_Data1; //cord X_ANGLE
cord=Y_Data1; //cord Y_ANGLE
}
void ACCEL_Z_Data(float *cord)
{
//g的标准值需要在水平情况下测量： G 16124
static float Z_ACCEL_Data;
static signed short S_F;
S_F=(IIC_Read(ACCEL_ZOUT_H)<<8)|(IIC_Read(ACCEL_ZOUT_L));
Z_ACCEL_Data=(float)S_F;
Z_ACCEL_Data/=16384.0; //cord Z
cord=Z_ACCEL_Data;
}
上面标为红色的部分哪位懂的大神给解释下吧？感激不尽呐
页:
[1]