YuanHongbin/LaserTracing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 新增激光定时关闭与姿态角持续输出测试功能，编码转UTF-8

Browse Source 
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:
YuanHongbin
2026-07-20 17:04:48 +08:00
parent 70758b1c06
commit 72844ff821
20 changed files with 1388 additions and 1260 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Module/Imu/Imu.c
+27 -27
View File
@@ -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)
}