Author: There is a fairy wife Xie the shopkeeper
Date: 2021/2/18
A series of small four-axis drones will be updated this year, from functional design → mind map → schematic design → PCB Layout → welding PCB → programming code → debugging of the whole machine to record their own growth process!
This small four-axis drone was made during the university years, and now I have a deeper understanding of embedded in work and study, so I want to reorganize the small four-axis, and then realize the flight control design of the large four-axis on this basis. , These will all be done after work!
//小四轴无人机设计，姿态解算
#include "atititude_calculation.h"
/*******************************************************************************
* fuction invSqrt
* brief 快速求解平方根的倒数
* param 求解的数
* return 求解结果
*******************************************************************************/
static 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;
}
/*******************************************************************************
* fuction UpdateQ_GetEurAngle
* brief 更新四元数，获取欧拉角
* param 陀螺仪值 加速度计值 更新周期
* return 无
*******************************************************************************/
void UpdateQ_GetEurAngle(float_XYZ *gryo,float_XYZ *accel,float dt)
{
float ax,ay,az;
float gx,gy,gz;
float recipNorm;
float halfvx,halfvy,halfvz;
float halfex,halfey,halfez;
/* 获取加速度值 */
ax = accel->X *100; //需要米每二次方秒 厘米每二次方秒 定高就很准
ay = accel->Y *100;
az = accel->Z *100;
/* 获取陀螺仪值 */
gx = gryo->X ;
gy = gryo->Y ;
gz = gryo->Z ;
/* 将陀螺仪的值转化为弧度 */
//1角速度 = 0.01745f
gx *=DEG_TO_RAD;
gy *=DEG_TO_RAD;
gz *=DEG_TO_RAD;
/* 将加速度的值归一化 */
recipNorm = invSqrt(ax*ax+ay*ay+az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
/* 提取四元数中的重力分量， */
halfvx = q1*q3 - q0*q2;
halfvy = q2*q3 + q0*q1;
halfvz = 0.5f - q1*q1 - q2*q2;
/* 求加速度的误差量 */
halfex = (ay*halfvz-az*halfvy);
halfey = (az*halfvx-ax*halfvz);
halfez = (ax*halfvy-ay*halfvx);
/* 误差量输入PID控制器 得到一个修正的角速度 */
gx += halfex*IMU_kp;
gy += halfey*IMU_kp;
gz += halfez*IMU_kp;
/* 用修正的角速度去更新四元数 */
q0 += 0.5f*dt*(-gx*q1-q2*gy-q3*gz);
q1 += 0.5f*dt*(gx*q0+q2*gz-q3*gy);
q2 += 0.5f*dt*(gx*q3+q0*gy-q1*gz);
q3 += 0.5f*dt*(gy*q1-q2*gx+q0*gz);
/* 四元数经过计算之后可能失去了规范性，因此需要在进行一次归一化 */
recipNorm = invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
/* 得到欧拉角 */
Eur.pitch = asinf(2.0f*(q0*q2-q1*q3))*RAD_TO_DEG;
Eur.roll = atan2f(2.0f*(q0*q1+q2*q3),q0*q0-q1*q1-q2*q2+q3*q3)*RAD_TO_DEG;
}
#ifndef _ATITITUDE_CALULATION_H__
#define _ATITITUDE_CALULATION_H__
#include "board_define.h"
#include "var_global.h"
#define DEG_TO_RAD 0.01745f
#define RAD_TO_DEG 57.29578f
void UpdateQ_GetEurAngle(float_XYZ *gryo,float_XYZ *accel,float dt);
#endif