feat: 新增激光定时关闭与姿态角持续输出测试功能，编码转UTF-8
1. 激光定时自动关闭：默认10分钟，laser on启动计时，超时自动关并上报 2. laser time <min>可配置定时分钟数，0=关闭定时 3. att on/off持续输出姿态角(Pitch/Roll/Yaw)，不注册不低功耗 4. 20个源文件从GB2312编码转为UTF-8 with BOM
This commit is contained in:
+27
-27
@@ -1,23 +1,23 @@
|
||||
/************************************************************************************************
|
||||
* 程序版本：V3.0
|
||||
* 程序日期：2022-11-3
|
||||
* 程序作者：
|
||||
/************************************************************************************************
|
||||
* 程序版本：V3.0
|
||||
* 程序日期：2022-11-3
|
||||
* 程序作者：
|
||||
************************************************************************************************/
|
||||
#include "Imu.h"
|
||||
#include "main.h"
|
||||
|
||||
#define Kp_New 0.9f //互补滤波当前数据的权重
|
||||
#define Kp_Old 0.1f //互补滤波历史数据的权重
|
||||
#define Acc_Gain 0.0001220f //加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0)
|
||||
#define Gyro_Gain 0.0609756f //角速度变成度 (初始化陀螺仪满量程+-2000 LSBg = 2*2000/65535.0)
|
||||
#define Gyro_Gr 0.0010641f //角速度变成弧度(3.1415/180 * LSBg)
|
||||
#define Kp_New 0.9f //互补滤波当前数据的权重
|
||||
#define Kp_Old 0.1f //互补滤波历史数据的权重
|
||||
#define Acc_Gain 0.0001220f //加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0)
|
||||
#define Gyro_Gain 0.0609756f //角速度变成度 (初始化陀螺仪满量程+-2000 LSBg = 2*2000/65535.0)
|
||||
#define Gyro_Gr 0.0010641f //角速度变成弧度(3.1415/180 * LSBg)
|
||||
|
||||
//kp=ki=0 就是完全相信陀螺仪
|
||||
//kp=ki=0 就是完全相信陀螺仪
|
||||
#define Kp 1.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
|
||||
//比例增益控制加速度计，磁力计的收敛速率
|
||||
//比例增益控制加速度计，磁力计的收敛速率
|
||||
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
|
||||
//积分增益控制陀螺偏差的收敛速度
|
||||
#define halfT 0.005f // half the sample period 采样周期的一半
|
||||
//积分增益控制陀螺偏差的收敛速度
|
||||
#define halfT 0.005f // half the sample period 采样周期的一半
|
||||
|
||||
static volatile float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
|
||||
static volatile float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
|
||||
@@ -29,7 +29,7 @@ static float MedianValue(float *value_buf, unsigned int size)
|
||||
unsigned int i,j;
|
||||
float temp;
|
||||
|
||||
//排序采用冒泡法
|
||||
//排序采用冒泡法
|
||||
for (j=0;j<=size;j++){
|
||||
for (i=0;i<=size-j;i++){
|
||||
if (value_buf[i] > value_buf[i+1]){
|
||||
@@ -194,48 +194,48 @@ static void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az
|
||||
if(ax*ay*az==0)
|
||||
return;
|
||||
|
||||
//加速度计<测量>的重力加速度向量(机体坐标系)
|
||||
//加速度计<测量>的重力加速度向量(机体坐标系)
|
||||
norm = invSqrt(ax*ax + ay*ay + az*az);
|
||||
ax = ax * norm;
|
||||
ay = ay * norm;
|
||||
az = az * norm;
|
||||
// printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);
|
||||
|
||||
//陀螺仪积分<估计>重力向量(机体坐标系)
|
||||
vx = 2*(q1q3 - q0q2); //矩阵(3,1)项
|
||||
vy = 2*(q0q1 + q2q3); //矩阵(3,2)项
|
||||
vz = q0q0 - q1q1 - q2q2 + q3q3 ; //矩阵(3,3)项
|
||||
//陀螺仪积分<估计>重力向量(机体坐标系)
|
||||
vx = 2*(q1q3 - q0q2); //矩阵(3,1)项
|
||||
vy = 2*(q0q1 + q2q3); //矩阵(3,2)项
|
||||
vz = q0q0 - q1q1 - q2q2 + q3q3 ; //矩阵(3,3)项
|
||||
|
||||
|
||||
//向量叉乘所得的值
|
||||
//向量叉乘所得的值
|
||||
ex = (ay*vz - az*vy);
|
||||
ey = (az*vx - ax*vz);
|
||||
ez = (ax*vy - ay*vx);
|
||||
|
||||
//用上面求出误差进行积分
|
||||
//用上面求出误差进行积分
|
||||
exInt = exInt + ex * Ki;
|
||||
eyInt = eyInt + ey * Ki;
|
||||
ezInt = ezInt + ez * Ki;
|
||||
|
||||
//将误差PI后补偿到陀螺仪
|
||||
//将误差PI后补偿到陀螺仪
|
||||
gx = gx + Kp*ex + exInt;
|
||||
gy = gy + Kp*ey + eyInt;
|
||||
gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
|
||||
gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
|
||||
|
||||
//四元素的微分方程
|
||||
//四元素的微分方程
|
||||
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
|
||||
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
|
||||
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
|
||||
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
|
||||
|
||||
//单位化四元数
|
||||
//单位化四元数
|
||||
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
|
||||
q0 = q0 * norm;
|
||||
q1 = q1 * norm;
|
||||
q2 = q2 * norm;
|
||||
q3 = q3 * norm;
|
||||
|
||||
// 矩阵表达式
|
||||
// 矩阵表达式
|
||||
// matrix[0] = q0q0 + q1q1 - q2q2 - q3q3; // 11
|
||||
// matrix[1] = 2.f * (q1q2 + q0q3); // 12
|
||||
// matrix[2] = 2.f * (q1q3 - q0q2); // 13
|
||||
@@ -246,7 +246,7 @@ static void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az
|
||||
// matrix[7] = 2.f * (q2q3 - q0q1); // 32
|
||||
// matrix[8] = q0q0 - q1q1 - q2q2 + q3q3; // 33
|
||||
|
||||
//下一步是四元数转换成欧拉角(Z->Y->X)
|
||||
//下一步是四元数转换成欧拉角(Z->Y->X)
|
||||
|
||||
}
|
||||
|
||||
|
||||
+6
-6
@@ -1,17 +1,17 @@
|
||||
#ifndef _IMU_H_
|
||||
#ifndef _IMU_H_
|
||||
#define _IMU_H_
|
||||
#include <math.h>
|
||||
|
||||
#define PI 3.14159265358979323846f
|
||||
#define RAD_TO_DEG 57.2957795 // 弧度到度的转换系数
|
||||
#define DEG_TO_RAD 0.0174533 // 度到弧度的转换系数
|
||||
#define RAD_TO_DEG 57.2957795 // 弧度到度的转换系数
|
||||
#define DEG_TO_RAD 0.0174533 // 度到弧度的转换系数
|
||||
|
||||
// 定义欧拉角结构体
|
||||
// 定义欧拉角结构体
|
||||
typedef struct {
|
||||
volatile float roll, pitch, yaw;//弧度
|
||||
volatile float roll, pitch, yaw;//弧度
|
||||
}Rad_T, *RadDp;
|
||||
typedef struct {
|
||||
volatile float roll, pitch, yaw;//角度
|
||||
volatile float roll, pitch, yaw;//角度
|
||||
}Deg_T, *DegDp;
|
||||
|
||||
#define Imu_AccFilterSize 10
|
||||
|
||||
Reference in New Issue
Block a user