【手搓固定翼fpv头追研究笔记】(二) 姿态解算及状态灯

序

前言已明姿器之驱，此文接以前之未成，成姿解之法，加之灯器

介绍

本文会补充完上篇未移植的madgwick算法，并且使用ws2812b作为状态显示

Madgwick：小头脑亦能解难题

姿态解算算法介绍

为什么要用madgwick而不用陀螺仪自带的dmp？
其一，madgwick对于mcu性能的要求偏低
其二，dmp只能对mpu6050的6轴数据解算，而我们要用到9轴数据融合

算法库的移植与魔改

关于Madgwick算法的具体介绍， 网上已经有很多了，比如这个，此处不做复述，网上所有的madgwick算法基本过程都一模一样，主要区别在与积分部分与单位转换的不同，不过都大差不差
madgwick的积分以微秒为单位在时间上进行积分，在我在网上冲浪的找到的大部分madgwick库（比如arduino的库还有各种51上面用的），都是需要一个固定的运行频率，但对于我们这个项目裸机开发而言，有些时候难以保证运行周期不变，在一阵瞎摸索之后，找到了这一个解决方式:

HAL_GetTick();

没错，就是这个函数，其返回值为一个32位无符号整数，系统自启动以来经过的毫秒数，那么我们只需要这样，就能得到积分所需的delta time，即dt：

uint32_t madgwick_timer=0;

float dt = (float)(HAL_GetTick() - madgwick_timer) / 1000.0f;
madgwick_timer = HAL_GetTick();

上述代码，将上一个解算周期时的系统毫秒数存储为madgwick_timer，而后与本周期进行时的毫秒数做差，除以1000得到微秒，而后对madgwick_timer进行更新，以此得到积分所需的dt

小tip：陀螺仪量程修改

在朋友的测试下，加速度计量程如果取±2g，那么可能会因突然的剧烈撞击导致姿态发生异常，所以我们需要加大陀螺仪量程为±8g：
经过查询寄存器定义可知，加速度计的配置寄存器地址为 0x1C，本寄存器的第四五位用于配置加速度量程，这两位00对应±2g，01对应±4g，10对应±8g，11对应±16g，所以我们需要将该寄存器配置为0001 0000，此时量程为±8g：

代码修改：
在mpu6050的初始化函数中能找到配置加速度计量程的代码：

MPU_I2C_Write(MPU6050_Write_Addr, MPU_Accel_Addr, (Set_Accel_Range << 3));

Set_Accel_Range就是范围的配置，这里我懒直接修改了这里的值而没有去修改宏定义（好孩子别学）：

MPU_I2C_Write(MPU6050_Write_Addr, MPU_Accel_Addr, (2 << 3)); 

这样量程就配置好了，但是在校准部分，我们z轴上减去的重力加速度值就不对了
当量程是±2g时，重力加速度1g对应值为 (0xffff>> 2)，而当量程为±8g时，因为量程扩大了4倍，所以重力加速度对应值需要再右移2位，即(0xffff >> 4)，具体代码如下，在MPU6050_calibrate(void)这个函数中，如此修改：

az_l = temp[2] / CL_cnt - (0xffff >> 2);

改为

az_l = temp[2] / CL_cnt - (0xffff >> 4);

OK，如此一来，基本准备都准备好，最后需要修改的就是单位的转化
对于加速度计：量程为±8g，那么 1g对应原始数据为（32768 / 8）=4096，原始数据转换为以g为单位即为
a = a_raw / 4096 即 a = a_raw * 0.000244
对于陀螺仪：量程为±2000°/s，灵敏度为：32767/2000 = 16.40 °/s，则
g = (g_raw / 16.40 °/s ) * 2*π / 360° 即 g = g_raw * 0.001064

废话不多说，直接上代码：（这里先使用6轴，磁力计的换算和数据融合放在磁力计校准那一张）
Madgwick.h:

#ifndef MADGWICK_H
#define MADGWICK_H
#include "main.h"

typedef struct AttiAgls{
   
    float roll;
    float pitch;
    float yaw;
}AttiAgls;

extern float Pitch, Roll, Yaw;
extern float q0, q1, q2, q3;

void MadgwickAHRSInit(void);
void MadgwickAHRSupdate_6(short ax_raw, short ay_raw, short az_raw, short gx_raw, short gy_raw, short gz_raw);
void MadgwickAHRSupdate_9(short ax_raw, short ay_raw, short az_raw, short gx_raw, short gy_raw, short gz_raw, short mx_raw, short my_raw, short mz_raw);

void Filter_Get_AttitudeAngles(AttiAgls *angles);

#endif //MADGWICK_H

Madgwick.c

#include "Madgwick.h"
#include <math.h>

#define beta 0.1f
#define GYRO_K 0.001064f
#define ACCEL_K 0.000244f
#define RAD_TO_DEG 57.295780f
#define DEG_TO_RAD 0.017453f

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

uint32_t madgwick_timer=0;

/**
 * @brief   启动姿态解算
 * */
void MadgwickAHRSInit(void)
{
   
    madgwick_timer = HAL_GetTick();
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root 来自《雷神之锤3》
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float number)
{
   
    long i;
    float x2, y;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    y  = number;
    i  = * ( long * ) &y;                       // evil floating point bit level hacking
    i  = 0x5f3759df - ( i >> 1 );               // what the fuck?
    y  = * ( float * ) &i;
    y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
//  y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

    return y;
}

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdate_6(short ax_raw, short ay_raw, short az_raw, short gx_raw, short gy_raw, short gz_raw)
{
   
    float ax, ay, az;
    float gx, gy, gz;
    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;

    //获取delta t用于积分
    float dt = (float)(HAL_GetTick() - madgwick_timer) / 1000.0f;
    madgwick_timer = HAL_GetTick();

    ax = (float)ax_raw * ACCEL_K;
    ay = (float)ay_raw * ACCEL_K;
    az = (float)az_raw * ACCEL_K;

    //将陀螺仪AD值转换为 rad/s
    gx = (float)gx_raw * GYRO_K;
    gy = (float)gy_raw * GYRO_K;
    gz = (float)gz_raw * GYRO_K;

    // 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
        recipNor